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21 世 纪 是 什么 样 的 世纪 ? 是 物 联 网 的 世纪 ? 是 VR 的 世纪 ? 也 许 吧 ， 但 我 更 相信 21 世 纪 是 机 器 人 的 世纪 。 


目前 ， 我 国 已 经 步 入 经 济 转 型 的 拐点 区 间 ， 人 口红 利 越 来 越 难 以 支撑 中 国 经 济 的 发 展 和 进步 。 在 很 多 行业 都 已 经 开始 了 机 器 
BHA, EP LEHRER KR. Lise. AMA, UREA RRS PAY MBA LARA) (2016 一 2020 年 ) 》。 这 份 规 
划 指 出 : “到 2020 年 ， 我 国 工 业 机 器 人 年 产量 达到 10 万 台 ， 其 中 六 轴 及 以 上 机 器 人 达到 5 万 台 以 上 ; 服务 机 器 人 年 销售 收入 超过 
300 亿 元 ， 在 助 者 、 助 残 、 医 疗 康 复 等 领域 实现 小 批量 生产 及 应 用 ; 要 培育 3 家 以 上 的 龙头 产业 ， 打 造 5 个 以 上 机 器 人 配套 产业 集 
群 ， 工 业 机 器 人 平均 无 故障 时 间 要 达到 8 万 小 时 ; 智能 机 器 人 实现 创新 应 用 。” 从 这 个 规划 中 可 以 看 出 ， 机 器 人 未 来 的 政策 空间 
和 市 场 发 展 空 间 都 是 非常 巨大 的 。 一 方面 ， 发 展 工 业 机 器 人 在 满足 我 国 制造 业 的 转型 升级 、 提 质 增 效 ， 实 现 “ 中 国 制 造 2025 等 
方面 具有 极为 重大 的 意义 ， 是 全 面 推进 实施 制造 强国 战略 的 重要 一 步 。 另 一 方面 ， 从 服务 机 器 人 来 说 ， 也 要 满足 未 来 市 场 需求 的 
增长 。 首 先 ， 这 包括 了 基本 生活 需求 ， 比 如 说 养老 、 助 老 、 助 残 等 。 其 次 是 国家 安全 需求 ， 比 如 救灾 、 抢 险 、 海 底 勘 探 、 航 天 、 
国防 。 最 后 还 有 家 庭 服务 和 娱乐 机 器 人 ， 比 如 娱乐 、 儿 童 教 育 、 智 能 家 居 应 用 等 。 同 时 ， 科 技 部 目前 也 在 进行 “十 三 五 ”科技 创 
新 规划 战略 研究 ， 根 据 已 经 披露 的 内 容 ， 其 中 对 机 器 人 (尤其 是 服务 机 器 人 ) 非常 重视 ， 并 会 在 近期 遵 选 并 局 动 一 批 相应 的 重大 
科技 项 目 。 


也 许 看 完 上 面 这 些 ， 你 会 觉得 这 更 像 是 新 闻 描 述 ， 机 器 人 产业 真 的 这 么 火爆 吗 ? 当然 。 即 使 创 除 工业 机 器 人 ， 只 谈 其 他 的 智 
能 机 器 人 或 服务 机 器 人 ， 这 也 是 一 个 相当 庞大 的 产业 。 例 如 ， 目 前 世界 上 最 大 的 服务 机 器 人 公司 应 该 还 是 美国 的 直觉 外 科 公 司 。 
他 们 生产 的 达 芬 奇 机 器 人 系统 在 全 世界 已 经 应 用 了 3000 余 套 ， 其 完成 的 手术 超过 千 万 例 。 由 机 器 人 完成 的 各 类 微 创 手术 让 无 数 患 
者 获得 新 生 。 这 家 公司 的 市 值 超过 300 亿 美元 。 中 国 目前 估 值 最 高 的 机 器 人 公司 是 大 疆 创 新 ， 其 年 营 收 额 已 经 超过 10 亿 美元 ， 估 
值 超过 百 亿 美元 。 要 知道 ，10 年 前 这 家 公司 刚刚 在 深圳 创立 时 ， 还 比较 弱小 。 因 此 ， 相 信 机 器 人 产业 在 未 来 的 前 景 一 定 非常 广 
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但 是 我 们 也 注意 到 在 军用 国防 、 救 灾 救 援 、 养 老 、 家 庭 服 务 、 儿 童 教育 等 领域 ， 没有 一 个 世界 级 的 机 器 人 公司 存在 。 有 
很 多 爱好 者 都 布 望 自己 能 够 制造 一 个 智能 轮椅 、 一 个 智和 人 或 是 一 个 家 庭 服务 机 器 人 ， 也 许 这 样 的 机 器 人 会 像 乔 布 
斯 和 沃 座 尼 亚 克 在 车 库 里 做 出 的 苹果 电脑 一 样 改变 世界 。 但 我 们 看 到 的 很 多 人 ， 自 己 只 是 机 械 工程 师 、 电 气 工 程 师 或 者 自动 控制 
工程 师 ， 开 发 一 个 控制 机 器 人 的 软件 系统 是 过 不 可 及 的 事情 ; 又 或 者 自己 虽然 是 软件 工程 师 ， 但 是 并 不 知道 如 何 控制 和 驱动 底层 
设备 。 这 要 怎么 办 呢 ? 没关系 ，ROS 机 器 人 操作 系统 可 以 帮忙 。 
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ROS 最 初 是 作为 科研 辅助 工具 由 斯 坦 福 大 学 开发 的 。 类 似 的 机 器 人 操作 系统 在 世界 各 国 还 有 很 多 。 有 些 操作 系统 面向 实时 机 
器 人 控制 ， 有 些 操作 系统 面向 机 器 人 仿真 ， 有 些 操作 系统 面向 用 户 交 互 。 这 些 操作 系统 大 部 分 都 相对 封闭 ， 各 成 体系 ， 没 有 在 学 
术 界 和 产业 界 造成 影响 。 而 由 于 ROS 极 大 的 开放 性 和 包容 性 ， 它 能 够 兼容 其 他 机 器 人 开发 工具 、 仿 真 工具 和 操作 系统 ， 使 之 融 为 
一 体 。 这 使 得 ROS 不 断 发 展 壮 大 ， 并 成 为 应 用 和 影响 力 最 广泛 的 机 器 人 软件 平台 。 


随 着 ROS2.0 的 开发 ，ROS 能 够 兼容 除 Linux 之 外 更 多 的 操作 系统 ， 如 Windows、Android; 能 够 支持 从 工业 计算 机 到 Adruino 开 
发 板 等 各 类 型 的 硬件 ; 能 够 采集 RGB-D 摄 像 头 、 普 通 摄像 头 和 各 种 类 型 的 传感器 数据 ; 能 够 驱动 类 人 形 机 器 人 、 四 轴 飞 行 器 等 
各 类 型 的 机 器 人 。 而 且 新 版 本 的 ROS 在 采用 SOA 架 构 的 基础 上 ， 集 成 了 MVC 框 架 ， 更 加 有 利于 机 器 人 人 机 交互 界面 的 开发 与 机 
器 人 控制 。 


学 习 ROS， 本 向 就 是 掌握 一 把 通 往 未 来 的 钥匙 。 自 从 本 人 2012 年 翻译 本 书 第 1 版 之 后 ， 关 于 ROS 的 书籍 也 渐渐 多 了 起 来 。 但 
和 其 他 书籍 相 比 ， 本 书 经 过 了 时 间 的 检验 ， 第 2 版 从 内 容 到 示例 对 学 习 ROS 都 更 有 帮助 。 和 希望 大 家 都 能 够 成 为 机 器 人 软件 设计 方 
面 的 专家 。 布 望 在 中 国 的 机 器 人 设计 领域 能 够 出 现 新 的 领军 人 物 ， 将 中 国 的 机 器 人 带 入 世界 一 流行 列 。 


刘 品 杰 ， 本 书 第 1 版 译 者 


Eth 


记得 第 一 次 接触 ROS 的 时 候 我 还 在 学 校 做 研究 ， 当 时 跟 一 些 海外 学 者 交流 时 得 知 有 这 个 专 为 机 器 人 设计 的 操作 系统 。 我 很 是 
兴奋 ， 一 心 想 把 ROS 用 在 我 们 最 新 研发 的 机 器 人 上 ， 于 是 就 马上 动手 玩 起 来 。 由 于 当时 ROS 尚 在 起 步 阶段 ， 说 明文 档 不 太 全 面 ， 
同时 社区 支持 又 很 少 ， 不 知道 经 过 多 少 折腾 才 好 不 容易 把 它 运行 起 来 。 体 验 后 发 现 它 的 设计 框架 确实 很 适合 作为 机 器 人 敏捷 开发 
工具 一 一 算法 及 控制 等 代码 很 容易 被 复 用 ， 避 免 了 很 乡 重 复 劳动 。 但 乏 何 当时 的 功能 包 不 多 ， 而 且 系统 对 运 工资 源 要 求 高 ， 最 终 


没有 用 在 当时 的 机 器 人 项 目 上 。 


由 于 其 开源 性 以 及 对 商用 友好 的 版 权 协 议 ，ROS 很 快 得 到 越 来 越 多 的 关注 及 支持 。 现 在 ROS 已 有 飞快 的 发 展 ， 越 来 越 多 机 器 
人 相关 的 软件 工具 亦 加 入 ROS 的 行列 。 国 外 一 些 商用 的 机 器 人 也 开始 支持 ROS 系 统 ， 其 至 基于 ROS 进 行 开 发 。 相 信 这 个 趋势 会 一 
直 持 续 下 去 并 会 蔓延 到 世界 各 地 。 而 我 亦 深 深 体 会 到 国内 对 ROS 的 关注 在 近年 有 显著 的 上 升 。 


从 前 在 国内 学 习 ROS 可 谓 孤 军 作战 ， 身 边 没 几 个 人 听 说 过 ROS， 而 且 只 能 从 国外 网 站 学 习 ROS 的 相关 知识 ， 完 全 没有 中 文 的 
资料 可 以 查看 。 幸 好 国内 有 不 少 有 心 人 积极 推动 ROS 的 发 展 ， 不 遗 余力 地 对 国外 ROS 相 关 的 文章 进行 翻译 ， 并 且 发 表 一 些 原创 的 


教学 文 鞋 ， 丰 富 ROS 的 中 文 资源 ， 这 使 学 习 ROS 变 得 更 方便 。 


我 与 本 书 译 者 通过 共同 举办 ROS 国 内 推广 课程 而 结缘 。 他 对 推动 ROS 在 国内 的 发 展 起 到 了 举足轻重 的 作用 ， 并 运营 着 国内 著 
名 的 ROS 交 流 社区 。 本 书 亦 是 他 贡献 KOS 中 文 社 群 的 作品 之 一 。 而 本 书 的 作者 同样 是 ROS 界 的 权威 ， 有 丰富 的 ROS 实 战 经验 ， 使 
用 ROS 进 行 过 多 种 机 器 人 的 开发 。 书 中 从 ROS 的 架构 概念 到 第 用 的 调试 工具 、 功 能 包 及 传感器 的 信息 处 理 都 有 所 涉及 ， 是 ROS 入 


门 必 看 书 之 一 。 和 项 望 本 书 能 让 你 快速 进入 ROS 的 世界 ， 探 索 ROS 的 精彩 。 
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机 器 人 对 于 现代 人 类 而 言 并 不 陌生 和 神秘 ， 它 在 百年 前 的 科幻 小 说 中 首次 出 现 ， 而 现在 已 经 逐步 进入 人 类 生活 的 方方面面 ， 
机 器 时 代 即 将 到 来 ! 
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的 程序 究竟 是 如 何 设 计 出 来 的 呢 ? 


智能 机 器 人 需要 具备 强健 的 “ 肢 、 明 亮 的 眼 、 灵 巧 的 “ 嘴 以 及 聪慧 的 “ 脑 ” ， 这 一 切 的 实现 实际 上 涉及 诸多 技术 领 
域 ， 需 要 艰辛 的 设计 、 开 发 与 调试 过 程 ， 必 然 会 遇 到 环 手 的 问题 和 挑战 。 而 一 个 小 型 的 开发 团队 难以 完成 机 器 人 各 个 方面 的 开发 
工作 ， 因 而 需要 一 套 合作 开发 的 框架 与 模式 ， 以 期 能 够 快速 集成 已 有 的 功能 ， 省 却 重复 劳动 的 时 间 。 旱 在 2008 年 ， 我 们 在 与 澳 大 
利 亚 的 布 劳 思 教授 交流 时 ， 就 得 知 他 们 开发 了 一 套 商业 化 的 “RoBIOS” 机 器 人 操作 系统 ， 这 套 系 统 将 一 些 常 用 的 机 器 人 底层 功 
能 进行 了 封装 ， 可 极 大 简化 高 级 功能 的 开发 。 据 他 们 介绍 ， 这 是 最 早 的 “机 器 人 操作 系统 ， 但 由 于 产品 不 开源 且 价 格 昂 贵 ， 我 


们 最 终 未 能 一 试 为 快 。 后 来 在 网 络 中 不 断 地 寻 砚 ， 最 终 发 现 了 ROS， 由 于 其 开源 、 开 放 的 特性 ， 一 下 子 就 引起 了 我 们 极 大 的 兴 
趣 。 


我 们 于 2010 年 建立 了 易 科 机 器 人 QQ 群 进行 讨论 ， 从 而 结识 了 国内 最 早期 的 一 些 机 器 人 研究 者 和 ROS 探 索 者 。 由 于 早期 相关 
资料 非常 项 乏 ， 我 们 于 2012 年 创建 了 博客 (blog.exbot.net) 以 进行 技术 分 享 与 交流 。 易 科 机 器 人 开发 组 成 员 在 此 期 间 贡 献 了 大 量 
的 教程 和 开发 笔记 ， 在 此 向 他 们 的 无 私 奉献 表示 感谢 与 敬意 ! 近年 来 ， 随 着 机 器 人 的 迅猛 发 展 ，ROS 得 到 了 更 为 广泛 的 使 用 ， 国 
内 也 出 现 了 一 些 优秀 的 项 目 ， 包 括 “ 星 火 计 划 ”ROS 公 开课 (blog.exbot.net/spatk) 、 “HandsFree ”ROS 机 器 人 开发 平台 


(wiki.exbot.net) 等 。 


出 版 界 近 年 来 也 是 硕果 累累 ， 术 书 第 1 版 便 是 国内 第 一 本 ROS 译 着， 由 于 实用 性 强 ， 已 经 多 次 重印 。 第 2 版 针对 近年 来 ROS 的 
最 新 发 展 ， 对 书 中 部 分 内 容 进行 了 修订 ， 并 增加 了 第 6 章 和 第 10 章 。 本 书 涵盖 了 使 用 ROS 进 行 机 器 人 编程 的 最 新 知识 与 方法 ， 通 
过 ROS 编 程 实践 能 够 帮助 你 理解 机 器 人 系统 设计 与 应 用 的 现实 问题 。 在 机 器 人 开发 实践 中 ,我们 认为 除了 成 功 的 喜悦 外 ， 还 看 到 
机 器 人 学 目前 所 处 的 发 展 阶段 : 核心 技术 尚未 成 熟 、 诸 多 功能 尚 不 完备 、bug 多 :…… 但 我 们 相信 ， 有 了 ROS 的 开源 精神 和 完备 的 
合作 开发 框架 ， 很 多 问题 会 迎刃而解 。 唯 一 迫切 需要 的 就 是 期 待 你 加 入 机 器 的 开发 和 研究 中 来 ， 一 起 推动 开源 机 器 人 技术 的 发 展 
与 首 及 ! 


木 书 第 2 版 与 第 1 版 的 重要 部 分 主要 沿用 了 刘 品 杰 在 第 1 版 中 的 翻译 ， 个 别 词汇 根据 习惯 进行 了 修改 。 具 体 来 说 ， 第 1 至 5 章 和 
第 10 章 由 张 瑞 雷 翻译 ， 第 6 章 由 张波 翻译 ， 第 7 至 9 章 由 刘 锦 涛 翻译 ， 吴 中 红 和 李 静 老师 对 全 书 进行 了 审阅 ， 最 后 由 刘 锦 涛 对 全 书 


进行 了 修改 润色 和 统 稿 整理 。 感 谢 杨 维 保 、 马 文科 等 人 对 本 书 提 出 的 修改 建议 ! 


我 们 将 会 在 books.exbot.net 发 布 本 书 的 其 他 相关 资源 。 


本 书 第 2 版 概括 性 地 介绍 了 ROS 系 统 的 各 种 工具 。ROS 是 一 个 先进 的 机 器 人 操作 系统 框架 ， 现 今 已 有 数 百 个 研究 团体 和 公司 
将 其 应 用 在 机 器 人 行业 中 。 对 于 机 器 人 技术 的 非 专业 人 士 来 说 ， 它 也 相对 容易 上 手 。 在 本 书 中 ， 你 将 了 解 如 何 安 装 ROS， 如 何 开 
始 使 用 ROS 的 基本 工具 ， 以 及 最 终 如 何 应 用 先进 的 计算 机 视觉 和 导航 工具 。 


在 阅读 本 书 的 过 程 中 无 需 使 用 任何 特殊 的 设备 。 书 中 每 一 章 都 附带 了 一 系列 的 源 代码 示例 和 教程 ， 你 可 以 在 自己 的 计算 机 上 
一 需要 做 的 事情 。 


运行 。 这 是 你 唯 
当然 ， 我 们 还 会 告诉 你 如 何 使 用 硬件 ， 这 样 你 可 以 将 你 的 算法 应 用 到 现实 环境 中 。 我 们 在 选择 设备 时 特意 选择 一 些 业余 用 户 


负担 得 起 的 设备 ， 同 时 涵盖 了 在 机 器 人 研究 中 最 典型 的 传感器 或 执行 器 。 


最 后 ， 由 于 ROS 系 统 的 存在 使 得 整个 机 器 人 上 有 具备 在 虚拟 环境 中 工作 的 能 力 。 你 将 学 习 如 何 创 建 自 己 的 机 器 人 并 结合 功能 强大 
的 导航 功能 包 集 。 此 外 如 果 使 用 Gazebo 仿 真 环境 ,你 将 能 够 在 虚拟 环境 中 运行 一 切 。 第 2 版 在 最 后 增加 了 一 章 ， 讲 如 何 使 
J “Move it! ”和 包 控 制 机 械 辟 执行 抓 取 任务 。 读 完 本 书后 ， 你 会 发 现 已 经 可 以 使 用 ROS 机 器 人 进行 工作 了 ， 并 理解 其 背后 的 原 


理 。 


主要 内 容 


第 1 章 介 绍 安装 ROS 系 统 最 简单 的 方法 ， 以 及 如 何在 不 同 平台 上 安装 ROS， 本 书 使 用 的 版 本 是 ROS Hydro。 这 一 草 还 会 说 明 


如 何 从 Debian 软 件 包 安装 或 从 源 代码 进行 编译 安装 ， 以 及 在 虚拟 机 和 ARM CPU 中 安装 。 


第 2 章 涉 及 ROS 框 架 及 相关 的 概念 和 工具 。 该 章 介绍 节点 、 主 题 和 服务 ， 以 及 如 何 使 用 它们 ， 还 将 通过 一 系列 示例 说 明 如 何 
调试 一 个 节点 或 利用 可 视 化 方法 直观 地 查看 通过 主题 发 布 的 消息 。 


第 3 章 进 一 步 展 示 ROS 强 大 的 调试 工具 ， 以 及 通过 对 节点 主题 的 图 形 化 将 节点 间 的 通信 数据 可 视 化 。ROS 提 供 了 一 个 日 志 记 
录 API 来 轻松 地 诊断 节点 的 问题 。 事 实 上 ， 在 使 用 过 程 中 ， 我 们 会 看 到 一 些 功 能 强大 的 图 形 化 工具 (orqt_console 和 rqt_oraph) , 
以 及 可 视 化 接口 (如 rqt_plot 和 rviz) 。 最 后 介绍 如 何 使 用 trosbag 和 和 tqt_bag 记 录 并 回放 消息 。 


第 4 章 介 绍 ROS 系 统 与 真实 世界 如 何 连 接 。 这 一 章 介绍 在 ROS 下 使 用 的 一 些 常见 传感器 和 执行 器 ， 如 激光 雷达 、 伺 服 电动 
机 、 摄 像 头 、RGB-D 传 感 器 、GPS 等 。 此 外 ， 还 会 解释 如 何 使 用 和 诅 入 式 系统 与 微 控制 器 (例如 非常 流行 的 Atduino 开 发 板 ) o 


第 5 章 介 绍 ROS 对 摄像 头 和 计算 机 视 沉 任务 的 支持 。 首 先 使 用 FireWite 和 USB 摄 像 头 驱动 程序 将 摄像 头 连接 到 计算 机 并 采集 图 
像 。 然 后 ， 你 就 可 以 使 用 ROS 的 标定 工具 标定 你 的 摄像 头 。 我 们 会 详细 介绍 和 说 明 什 么 是 图 像 管道 ， 学 习 如 何 使 用 集成 了 
OpenCV 的 多 个 机 器 视觉 API。 最 后 ， 安 装 并 使 用 一 个 视觉 里 程 计 软 件 。 


第 6 章 将 展示 如 何在 ROS 节 点 中 使 用 点 云 库 。 该 章 从 基本 功能 入 手 ， 如 读 或 写 PCL 数 据 片 段 以 及 发 布 或 订阅 这 些 消息 所 必需 
的 转换 。 然 后 ， 将 在 不 同 节点 间 创 建 一 个 管道 来 处 理 3D 数 据 ， 以 及 使 用 PCL 进 行 缩 减 采 样 、 过 滤 和 搜索 特征 点 


第 7 章 介绍 在 ROS 系 统 中 实现 机 器 人 的 第 一 步 是 创建 一 个 机 器 人 模型 ， 包 括 在 Gazebo 仿 真 环境 中 如 何 从 头 开 始 对 一 个 机 器 人 
行 建 模 和 仿真 ， 并 使 其 在 仿真 环境 中 运行 。 你 也 可 以 仿真 摄像 头 和 激光 测 距 传感器 ， 为 后 续 学 习 如 何 使 用 ROS 的 导航 功能 包 集 
和 其 他 工具 英 定 基础 。 


8 章 是 两 章 关 于 ROS 导 航 功 能 包 集中 的 第 1 章 。 该 章 介绍 如 何 对 你 的 机 器 人 进行 使 用 导航 功能 包 集 所 需 的 初始 化 配置 。 然 后 
用 几 个 例子 对 导航 功能 包 集 进 行 说明 。 


第 9 章 延 续 第 8 章 的 内 容 ， 介 绍 如 何 使 用 导航 功能 包 集 使 机 器 人 有 效 地 上 自主 导航 。 该 章 介 绍 使 用 ROS 的 Gazebo 仿 真 环 境 和 rviz 
创建 一 个 虚拟 环境 ,在 其 中 构建 地 图 、 定 位 机 器 人 并 用 障碍 回避 做 路 径 规 划 。 


第 10 章 讨论 ROS 中 移动 机 器 人 机 械 辟 的 一 个 工具 包 。 该 章 包 含 安装 这 个 包 所 需要 的 文档 ， 以 及 使 用 Movelt! 操作 机 械 导 进行 


抓 取 、 放 置 ， 简 单 的 运动 规划 等 任务 的 演示 示例 。 


预备 知识 


我 们 写作 本 书 的 目的 是 让 每 位 读者 都 可 以 完成 本 书 的 学 习 并 运行 示例 代码 。 基 本 上 ， 你 只 需要 在 计算 机 上 安装 一 个 Linux 发 
行 版 。 虽 然 每 个 Linux 发 行 版 应 该 都 能 使 用 ,但 还 是 建议 你 使 用 Ubuntu12.04LTS。 这 样 你 可 以 根据 第 1 章 的 内 容 安 装 ROS Hydro。 


对 于 ROS 的 这 一 版 本 ， 你 将 需要 Ubuntu14.04 之 前 的 版 本 ， 因 为 之 后 的 版 本 已 经 不 再 支持 Hydto 了 。 


对 于 硬件 要 求 ， 一 般 来 说 ， 任 何人 台式 计 算 机 或 笔记 本 电脑 都 满足 。 但 是 ， 最 好 使 用 独立 显卡 来 运行 Gazebo 仿 真 环境 。 此 外 ， 
如 果 有 足够 的 外 围 接 口 将 会 更 好 ， 因 为 这 样 你 可 以 连接 几 个 传感器 和 执行 器 ， 包 括 摄 像 头 和 Arduino 开 发 板 。 


你 还 需要 Git (git-core Debian 软 件 包 ) ， 以 便 从 本 书 提 供 的 源 代 码 中 复制 库 。 同 样 ， 你 需要 具备 Bash 命 令 行 、GNUVLinux 工 
有 具 的 基本 知识 和 一 些 C/C++ 编 程 技巧 。 


目标 读者 


本 书 的 目标 读者 包括 所 有 机 器 人 开发 人 员 ， 可 以 是 初学 者 也 可 以 是 专业 人 员 。 它 涵盖 了 整个 机 器 人 系统 的 各 个 方面 ， 展 示 了 
ROS 系 统 如 何 帮助 完成 使 机 器 人 真正 自主 化 的 任务 。 对 于 听 说 过 却 从 未 使 用 过 ROS 的 机 器 人 专业 学 生 或 科研 人 员 来 说 ， 本 书 将 是 
非常 有 益 的 。ROS 初 学 者 能 从 本 书 中 学 习 ROS 软 件 框架 的 很 多 先进 理念 和 工具 。 不 仅 如 此 ， 经 常 使 用 ROS 的 用 户 也 可 能 从 菜 些 章 


节 中 学 习 到 一 些 新 东西 。 当 然 ， 只 有 前 3 章 是 纯粹 为 初学 者 准备 的 ， 所 以 那些 已 经 使 用 过 ROS 的 人 可 以 跳 过 这 部 分 直接 阅读 后 面 
的 章节 。 


源 代码 和 彩色 图 片 下 载 


可 以 从 你 在 http://www.packtpub.com 中 的 账户 下 载 所 有 已 购买 的 Packt Publishing 出 版 的 书籍 的 示例 代码 文件 。 如 果 你 在 其 他 
地 方 购买 了 这 本 书 ， 可 以 访问 http://www.packtpub.com/support 并 注册 ， 文 件 会 通过 电子 邮件 直接 发 送 给 你 。 还 可 以 
从 https://github.com/AaronMR/ROS_Book_Hydro 下 载 源 代码 文件 。 


我 们 同时 提供 了 本 书 所 有 彩色 的 屏幕 截图 、 对 话 框 的 PDF 文件 ， 这 些 彩 色 图 片 能 够 更 好 地 帮助 你 理解 输出 的 变化 。 可 以 
从 http://www.packtpub.com/sites/default/files/downloads/7580OS_ColorImages.pdf 下 载 这 个 文件 。 


作者 简介 


Enrique Fetnandez 在 拉 斯 帕尔马 斯 大 学 获得 计算 机 工程 博士 学 位 ， 目 前 是 Cleatpath Robotics 公 司 高 级 机 器 人 工程 师 。2009 年 他 
完成 了 关于 SLAM 的 硕士 学 位 论文 。2013 年 他 的 博士 论文 解决 了 自主 水 下 消 翔 器 (AUG) 的 路 径 规 划 问 题 。 那 段 时 间 ， 他 还 研究 
了 计算 机 视 党 、 人 工 智 能 以 及 其 他 机 器 人 学 课题 ， 例 如 赫 罗 纳 大 学 的 CIRS/ViCOROB 研 究 实 验 室 AUV 的 惯性 导航 系统 和 视觉 
SLAM。 他 在 2012 年 参加 了 欧洲 学 生 自 主 水 下 航行 器 设计 挑战 赛 (Student Autonomous Underwater Challenge-Europe, SAUC-E) 并 
获奖 ， 在 2013 年 作为 合作 者 参与 了 SAUC-E。 
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获得 博士 学 位 后 ，Enrique 作 为 高 级 机 器 人 工程 师 在 2013 年 6 月 加 入 PAL Robotics 公 司 的 自主 导航 部 门 。 在 那里 ， 他 开发 了 用 
于 REEM、REEM-C 和 移动 机 器 人 以 及 相关 项 目的 软件 ， 如 使 用 ROS 框 架 的 Stockbot。 他 的 研究 方向 包括 运动 规划 (路径 规划 和 
移动 机 器 人 控制 ) 、 机 器 人 定位 和 SLAM。 在 2015 年 ， 他 作为 高 级 自主 系统 开发 人 员 加 盟 Clearpath Robotics 公 司 的 自主 系统 部 门 从 
事 SLAM 相 关 工 作 。 


在 学 术 方 面 ，Enrique 发 表 了 多 篇 会 议论 文 ， 其 中 两 篇 于 2011 年 发 表 在 International Conference of Robotics and Automation» 
(ICRA) 上 。 他 是 Packt Publishing 出 版 的 第 1 版 《ROS 机 器 人 程序 设计 》 和 其 他 一 些 书 部 分 章节 的 作者 。 他 的 硕士 学 位 论文 是 关 
于 室内 机 器 人 的 FastSLAM 算 法 ， 此 机 器 人 装备 了 SICK 激 光 扫 描 仪 以 及 Pioneer 差 动 平台 的 轮 式 里 程 计 。 他 的 博士 学 位 论文 是 关于 
AUG 的 路 径 规 划算 法 和 工具 。 他 还 拥有 电子 和 时 入 式 系 统 (如 PC104 和 Atduino) 的 开发 经 验 。 他 的 研究 背景 包括 SLAM、 计 算 机 


视 沈 、 路 径 规 划 、 优 化 、 机 器 人 学 和 人 工 智 能 。 


我 要 感谢 这 本 书 的 合 著者 ， 感 谢 他 们 为 完成 这 本 书 所 付出 的 努力 以 及 提供 了 无 数 示例 的 代码 。 我 还 要 感谢 博士 论文 期 间 大 学 
智能 系统 和 计算 工程 研究 所 (University Institute of Intelligent Systems and Computational Engineering, SIANI) 和 水 下 机 器 人 研究 中 
心 (Center of Underwater Robotics Research, CIRS/ViCOROB) 的 研究 小 组 成 员 。 我 也 要 感谢 在 PAL 机 器 人 公司 的 同事 ， 在 那里 我 
学 到 很 多 关于 ROS、 机 器 人 运动 以 及 仿 人 双 足 机 器 人 的 知识 ， 不 仅 有 软件 ， 还 有 电子 和 硬件 设计 。 最 后 ， 我 要 感谢 我 的 家 人 和 朋 
友 的 帮助 与 支持 。 


Luis Sanchez Ctespo 在 拉 斯 帕尔马 斯 大 学 获得 了 电子 与 电信 工程 的 双 硕 士 学 位 。 他 曾 在 技术 开发 和 创新 研究 所 (IDETIC) 、 
加 那 利 群岛 海洋 平台 (PLOCAN) 和 应 用 微 电 子 研究 所 (IUMA) 与 不 同 的 研究 小 组 合作 ， 进 行 超 分 辨 率 算 法 成 像 研究 。 


他 的 专业 兴趣 包括 应 用 于 机 器 人 系统 的 计算 机 视觉 、 信 号 处 理 和 电子 设计 。 因 此 ， 他 加 入 了 AVORA 团 队 ， 这 批 年 轻 的 工程 
师 和 学 生 从 零 开始 从 事 自主 水 下 航行 器 (AUV) 的 开发 工作 。 在 这 个 项 目 中 ，Luis 开 始 开发 声学 和 计算 机 视 党 系统 ， 用 于 提取 不 
同 传感器 的 信息 ， 例 如 水 听 器 、 上 声响 和 摄像 头 。 


依托 海洋 技术 的 强大 背景 ，Luis 与 人 合作 创办 了 一 家 新 的 初创 公司 Subsea Mechatronics, 27 FART RIF Aik IE EME Fe A 


ve 


主 航行 器 。 


下 面 是 海洋 技术 工程 师 和 企业 家 (LPA Fabrika: Gran Canaria Maker Space 的 联合 创始 人 和 制造 商 ) Dario Sosa Cabteta 对 Luis 的 


评价 : 


“他 很 热情 ， 是 一 个 多 学 科 的 工程 师 。 他 对 工作 负责 。 他 可 以 自我 管理 ， 并 承担 一 个 团队 领导 者 的 责任 ， 如 在 SAUC-E 竞 赛 
中 领导 了 AVORA 团 队 。 他 在 电子 和 电信 和 领域 的 背景 让 其 具备 从 信号 处 理 和 软件 到 电子 设计 和 制造 的 广泛 专业 知识 。 


Luis 作 为 技术 审 校 者 参与 了 Packt Publishing 出 版 的 第 1 版 《ROS 机 器 人 程序 设计 》 的 相关 工作 。 


首先 ， 我 要 感谢 Aaron、Anil 以 及 Entique 邀 请 我 参与 编写 这 本 书 。 和 他 们 一 起 工作 非常 快乐 。 同 时 ， 我 也 要 感谢 水 下 机 电 团 
队 关 于 重型 水 下 机 器 人 的 丰富 经 验 ， 这 些 年 我 们 一 起 成 长 。 我 必须 提 到 LPA Fabrika: Gran Canaria Maker Space， 他 们 备课 和 讲授 
教育 机 器 人 及 技术 项 目 极 富 热 情 ， 与 他 们 共享 工作 环境 也 非常 令 人 激动 。 


最 后 ， 我 要 感谢 我 的 家 人 和 女 朋 友 对 我 参与 的 每 个 项 目的 大 力 支 持 和 鼓励 。 


Anil Mahtani 是 一 名 从 事 水 下 机 器 人 工作 5 年 的 计算 机 科学 家 。 他 第 一 次 在 该 领域 工作 是 在 完成 硕士 论文 期 间 为 低 成 本 ROV 开 
发 软件 架构 。 在 此 期 间 ， 他 也 成 为 AVORA 的 团队 领导 者 和 主要 开发 人 员 ， 这 个 团队 的 高 校 学 生 设 计 和 开发 了 一 个 自主 水 下 航行 
器 并 参加 了 2012 年 的 SAUC-E。 同 年 ， 他 完成 了 论文 并 获得 了 拉 斯 帕 尔 蕊 斯 大 学 的 计算 机 科学 硕士 学 位 。 此 后 不 久 ， 他 成 为 
SeeByte 公 司 的 软件 工程 师 ， 这 家 公司 是 水 下 系统 智能 软件 解决 方案 的 全 球 领 导 者 。 


在 SeeByte 公 司 工 作 期 间 ，Anil 参 与 了 军 方 、 石 油 和 天 然 气 公司 的 一 些 半 自主 和 自主 水 下 系统 的 核心 开发 。 在 这 些 项 目 中 ， 
积极 参与 自主 系统 开发 、 分 布 式 软件 体系 结构 设计 和 底层 软件 开发 ， 同 时 也 为 前 视 声 呐 图 像 提 供 计 算 机 视觉 解决 方案 。 他 还 获得 
了 项 目 经 理 职位 ， 管 理 一 个 开发 和 维护 内 核 C++ 库 的 工程 师 团 队 。 


他 的 专业 兴趣 主要 包括 软件 工程 、 算 法 、 分 布 式 系统 、 网 络 和 操作 系统 。Anil 在 机 器 人 方向 主要 负责 提供 高 效 和 健壮 的 软件 
解决 方案 ， 不 仅 解 决 当 前 存在 的 问题 ， 还 预见 未 来 的 问题 或 可 能 的 改进 。 鉴 于 他 的 经 验 ， 他 在 计算 机 视觉 、 机 器 学 习 和 控制 问题 
上 也 有 独特 的 见解 。Anil 对 DIY 和 电子 学 有 兴趣 ， 并 且 开 发 了 一 些 Atduino 库 回馈 社区 。 


首先 ， 我 要 感谢 我 的 家 人 和 朋友 的 支持 ， 他 们 总 是 在 我 最 需要 的 时 候 帮 助 我 。 我 也 要 感谢 我 的 同事 和 朋友 David Rubio 
Vidal, Emilio Miguelafiez Martin#eJohn Brydon 给 我 最 大 的 支持 ， 他 们 以 专业 的 方式 教 我 很 多 知识 。 我 还 要 感谢 我 在 SeeByte 和 
AVORA 团 队 的 同事 ， 这 些 年 从 他 们 那里 学 习 并 经 历 很 多 。 最 后 ， 我 要 特别 感谢 Joree Cabrera Gimez， 他 的 指导 和 建议 成 就 了 我 自 
己 从 未 想象 到 的 职业 生涯 。 


Aaron Martinez 是 数字 化 制造 领域 的 电脑 工程 是、 企业 家 和 和 专家。 他 于 2010 年 在 拉 斯 帕尔马 斯 大 学 的 Instituto Universitario de 
Cienciasy Tecnologias Ciberneticas (IUCTC) 完成 硕士 论文 。 他 在 远程 监控 领域 使 用 沉浸 式 设备 和 机 器 人 平台 准备 硕士 论文 。 得 到 
学 位 后 ， 他 参加 了 在 奥地利 林 英 开 首 勒 大 学 研究 所 的 机 器 人 学 实习 计划 。 在 实习 期 间 ， 他 作为 团队 的 一 员 使 用 ROS 和 导航 包 集 进 
行 移动 平台 开发 。 之 后 ， 他 参与 了 有 关机 器 人 的 项 目 ， 其 中 一 个 是 拉 斯 帕尔马 斯 大 学 的 AVORA 项 目 。 在 这 个 项 目 中 ， 他 参与 自 
主 水 下 航行 器 制作 ， 并 参与 意大利 的 SAUC-E。2012 年 ， 他 负责 维护 这 个 项 目 ; 2013 年 ， 他 帮助 从 ROS 向 机 器 人 平台 移植 导航 包 


集 和 其 他 算法 。 


最 近 ，Aaton 与 人 共同 创立 了 一 家 名 为 SubSeaMechattonics SELL 的 公司 。 这 家 公司 从 事 与 水 下 机 器 人 和 遂 控 系统 相关 的 项 目 ， 还 
设计 和 制造 水 下 传感器 。 人 公司 的 主要 目标 是 开发 用 于 研发 原型 和 重型 机 械 手 的 定制 解决 方案 。 


Aaton 有 许多 领域 的 经 验 ， 比 如 编程 、 机 器 人 人、 机电 一 体 化 、 数 字 化 制造 以 及 Atrduino、BeagleBone、 服 务 器 和 激光 雷达 等 设 


备 。 如 今 ， 他 在 SubSeaMechatronics SL 公司 从 事 水 下 和 空中 环境 的 机 器 人 平台 设计 。 


我 要 感谢 我 的 女 朋友 ， 她 在 我 写 这 本 书 时 给 我 支持 以 及 给 我 继续 成 长 的 动力 。 我 还 要 感谢 Donato Monopoli (加 那 利 群岛 技术 
研究 所 (ITC) 生物 医学 工程 部 门 的 主管 ) ， 以 及 ITC 所 有 的 工作 人 员 ， 感 谢 他 们 使 我 懂得 数字 制造 、 机 械 以 及 组 织 工 程 ， 我 在 
此 度 过 了 生命 中 最 美好 的 时 光 。 


感谢 我 大 学 的 同事 ， 特 别 是 Alexis Quesada， 他 给 了 我 在 准备 硕士 论文 时 创建 我 第 一 个 机 器 人 的 机 会 。 和 他 们 一 起 工作 ， 使 我 


学 习 到 很 多 关于 机 器 人 的 知识 。 


最 后 ， 我 要 感谢 家 人 和 朋友 的 帮助 与 支持 。 


Piotr Gtodek 是 一 位 对 计算 机 视 党 和 图 像 处 理 感 兴趣 的 C++ 程序 员 。 他 曾经 是 艇 入 式 程序 员 ， 现 在 工作 于 银行 。 他 是 开源 游 


戏 和 无 人 驾驶 汽车 的 开发 者 。 空 闲 时 间 他 喜欢 跑步 、 打 壁球 和 阅读 。 


Akihiko HONDA 是 空间 机 器 人 工程 师 。 他 于 2012 年 在 东京 工业 大 学 完成 了 硕士 论文 。 目 前 他 正在 东京 工业 大 学 进行 博士 课 


程 学 习 。 


他 的 研究 兴趣 包括 与 空间 机 器 人 的 远程 操作 和 自动 化 相互 对 应 的 灵活 或 可 变形 的 材料 。 他 的 一 个 目标 是 通过 开发 更 好 的 自动 
控制 系统 提高 飞船 在 太空 的 性 能 和 稳定 性 。 在 早期 的 研究 中 ， 他 参与 了 包含 大 型 太阳 能 电池 阵列 和 用 于 捕捉 国际 空间 站 的 空间 机 
械 尼 的 地 球 观测 卫星 的 工作 。 目 前 ， 他 计划 将 其 研究 成 果 应 用 于 空间 太阳 能 电力 系统 、 行 星 探测 车 等 。 他 作为 最 佳 参赛 人 获奖 ， 
并 在 日 本 天 文学 会 举办 的 JSF 卫 星 设计 竞赛 中 因 提 出 使 用 其 研究 的 一 个 新 型 探索 宇宙 飞船 而 获奖 。 


他 在 大 学 的 研究 过 程 中 ， 还 参与 了 由 日 本 太空 发 展 署 指挥 的 一 些 项 目 。 在 JEM (REX-J) 项 目的 机 器 人 实验 中 ， 他 参与 了 在 
轨道 上 的 实验 设备 支持 操作 并 在 研究 中 获得 灵感 。 他 还 参加 了 一 个 为 宇航 员 开 发 可 穿戴 机 械 辟 的 项 目 并 开发 了 手动 控制 系统 。 目 
前 他 正 工 作 于 两 个 探索 机 器 人 项 目 。 其 中 一 个 是 开发 名 为 “KENAGE” 的 可 变形 的 探测 车 ， 用 于 克服 月 球 和 火星 特殊 的 崎 哎 地 
形 。 这 个 探测 车 正在 使 用 GAZEBO 仿 真 器 进行 可 行 性 实验 测试 。 在 另 一 个 项 目 中 ， 他 为 JumpingScouter 开 发 了 一 个 环境 识别 系 


统 。 


2013 年 ， 他 参与 了 萨 利 大 学 的 SMART 探 测 器 项 目 ， 开 发 环境 保护 和 识别 系统 。 同 时 ， 他 也 参与 了 探测 车 在 真实 环境 中 检测 


实际 功能 的 现场 实验 。 


我 要 感谢 JAXA 的 Hiroki KATO 给 我 打开 了 ROS 的 大 门 并 对 我 的 研究 给 予 宝 贵 建议 。 我 还 要 感谢 Mitsushige ODA 教 授 、Hiroki 
NAKANISHI 教 授 以 及 东京 工业 大 学 空间 机 器 人 实验 室 的 同事 。 他 们 分 享 了 空间 机 器 人 的 美好 前 景 ， 提 供 了 建议 ， 并 支持 我 在 研 
究 中 使 用 ROS 实 现 它们 。 我 也 要 感谢 萨 利 大 学 STAR 实验 室 的 教授 和 同事 给 我 提供 在 真实 环境 中 使 用 ROS 的 重要 建议 。 我 特别 感 


谢 来 自 大 加 那 利 岛 的 朋友 给 我 介绍 这 令 人 振奋 的 工作 。 
最 后 ， 我 要 感谢 我 的 家 人 Yoshihiko、Nobuko 和 Ayaka 对 我 的 生活 和 梦想 的 支持 ， 同 时 感谢 女 朋 友 对 我 的 理解 。 


Matthieu Kellet 是 一 位 热爱 技术 和 计算 机 科学 的 法 国 工 程 师 。 他 接受 的 教育 涉及 计算 和 机 器 人 学 ， 这 使 他 成 为 一 名 爱好 者 。 
他 审 校 了 本 书 的 第 1 版 。 


Aridane J.Sarrionandia de Leon 研 究 计 算 机 科学 ， 并 对 机 器 人 学 和 自主 航行 器 有 非常 大 的 兴趣 。 他 的 学 位 课题 是 关于 使 用 声呐 
的 水 下 地 图 构建 ， 为 此 他 在 自主 水 下 航行 器 中 使 用 ROS 工 作 。 他 有 自主 系统 和 ROS 的 经 验 。 他 熟悉 OpenCV 和 PCL， 目 前 正在 开 
发 自主 水 面 航行 器 的 控制 系统 。 


我 要 感谢 Luis 和 Aaron 给 我 这 个 机 会 审 校 这 本 书 。 同 时 ， 我 要 感谢 拉 斯 帕尔马 斯 大 学 的 AVORA 团 队 ， 特 别 是 Aaron、Luis 和 
Enrique， 他 们 给 我 介绍 了 ROS 的 神奇 之 处 ， 并 帮助 我 探索 自主 航行 器 世界 。 感 谢 我 的 导师 Jorge Cabrera Gimez， 他 让 我 有 机 会 成 
为 AVORA 团 队 的 一 员 。 


最 后 ， 我 要 感谢 我 的 家 人 和 朋友 在 我 生活 中 出 现 问题 时 支持 我 ， 特 别 要 感谢 Eva 纠正 我 不 清晰 的 语句。 


第 1 章 ROS Hydro 系统 入 门 


欢迎 开始 阅读 本 书 第 1 草 。 本 草 将 介绍 如 何 安 六 ROS 系 统 ， 它 是 一 种 新 的 标准 化 机 器 人 系统 软件 框架 。 本 书 是 基于 ROS 
Fuerte 的 《ROS 机 器 人 程序 设计 》 一 书 的 升级 版 。 通 过 ROS， 你 可 以 使 用 大 量 的 示例 代码 和 开源 程序 轻松 地 完成 机 器 人 编程 和 
控制 。 同 时 ， 你 还 能 够 理解 如 何 使 用 各 种 传感器 与 执行 器 ， 并 为 你 的 机 器 人 增加 新 的 功能 ， 如 目 动 导航 和 视 党 感知 等 。 得 益 于 开 
源 理念 ， 以 及 持续 开 友 最 先进 算 冯 并 不 断 提供 新 功能 的 开源 社区 ，ROS 不 断 进 步 完善 。 


通过 本 书 ， 你 将 学 习 到 如 下 内 容 : 

在 特定 版 本 的 Ubuntu 系 统 下 安装 ROS Hydro 框架 
- ROS 的 基本 操作 

` 调试 以 及 数据 可 视 化 

- 在 ROS 框 架 下 进行 机 器 人 编程 

连接 传感器 、 执 行 器 和 硬件 设备 以 创建 机 器 人 


. 创造 三 维 GD) 模型 并 进行 仿真 


本 章 主 要 介绍 怎样 在 Ubuntu 系 统 中 安装 完整 版 本 的 ROS Hydro。Ubuntu 不 但 能 够 全 面 支持 ROS， 而 且 是 ROS 官 方 推荐 的 
操作 系统 。 当 然 ， 你 也 可 以 在 其 他 的 操作 系统 中 安 疼 ROS。 这 本 书 使 用 的 Ubuntu 版 本 是 12.04 (Precise Pangolin) ， 你 可 以 
在 http://releases.ubuntu.com/12.04/ 免 费 下 载 安装 。 


在 开始 安装 之 前 ,我 们 首先 了 解 一 下 ROS 的 历史 。 


Robot Operating System (ROS) 是 一 个 得 到 广泛 使 用 的 机 器 人 系统 的 软件 框架 。ROS 的 基本 原理 是 无 需 改 动 束 能 够 在 不 
同 的 机 器 人 上 复 用 代码 。 基 于 这 些 ， 我 们 就 可 以 在 不 同 的 机 器 人 上 分 享 和 复 用 已 经 实现 的 功能 ， 而 不 需要 做 太 多 的 工作 ， 避 免 了 


重复 劳动 。 


2007 年 ， 斯 坦 福 大 学 人 工 智 能 实验 军 (Stanford Artificial Intelligence Laboratory, SAIL) 在 斯 坦 福 Al 机 器 人 项 目 
(Stanford Al Robot project) 的 支持 下 开发 了 ROS。2008 年 之 后 ， 其 主要 在 Willow Garage 公 司 支 持 下 与 超过 20 多 家 研究 机 
构 联合 研发 ROS。 


现在 已 经 有 很 多 家 研究 机 构 通 过 增加 ROS 支 持 的 硬件 或 开放 软件 源 代码 的 方式 加 入 ROS 系 统 的 开 友 中 。 同 样 ， 也 有 很 多 家 公 
司 将 其 产品 逐步 进行 软件 迁移 并 在 ROS 系 统 中 应 用 。 一 些 完全 支持 ROS 系 统 的 平台 如 下 图 所 示 。 这 些 平台 往往 会 开放 大 量 的 代 
码 、 示 例 和 仿真 环境 ， 以 便 开 友 人 员 轻 松 地 开展 工作 。 前 三 个 发 布 代码 的 机 器 人 例子 是 人 形 机 器 人 。 最 后 一 个 是 由 拉 斯 帕尔马 斯 
大 学 开发 的 水 下 机 器 人 ， 代 码 尚未 公布 。 你 可 以 在 http://wiki.ros.org/Robots 找 到 很 多 这 样 的 例子 。 
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ROS 系 统 已 经 支持 这 些 机 器 人 中 的 传感器 和 执行 器 ， 同 时 每 天 ROS 软 件 框 架 支持 的 设备 也 在 增加 。 此 外 ， 得 益 于 ROS 和 开放 
硬件 ， 大 量 公司 正在 创建 更 便宜 和 更 强大 的 传感器 。Arduino 开 上 友 板 是 一 个 很 好 的 例子 ， 使 用 廉价 的 电路 板 可 以 添加 很 多 类 型 的 
传感器 如 编码 器 、 光 和 温度 传感器 等 。 


; 


ROs 提 供 了 一 个 标准 的 操作 系统 环境 ， 包 括 硬 件 抽象 、 底 层 设 备 控 制 、 通 用 功能 的 实现 、 进 程 间 消息 转 友和 功能 包 管 理 等 。 


它 基于 一 个 集中 式 拓扑 的 图 结构 ， 在 节点 中 接收 许多 传感器 、 控 制 、 状 态 、 规 划 、 执 行 器 数据 进行 计算 处 理 ， 并 发 送 。 它 的 
各 种 库 与 功能 包 都 是 面向 类 UNIX 系 统 的 。 


*-ros-pkg 作 为 一 种 社区 化 的 软件 库 使 开发 高 级 库 更 为 容易 。 其 中 ， 很 多 功能 是 和 ROS 系 统 绑 定 的 ， 如 导航 库 和 rviz 可 视 化 
界面 都 基于 这 个 库 。 其 中 的 一 些 库 包 含 很 多 强大 的 工具 ， 可 以 帮助 我 们 方便 使 用 ROS 并 了 解 机 器 人 当前 的 实时 状态 。 其 中 ， 可 视 
化 工具 、 仿 真 环境 和 调试 工具 是 最 重要 的 几 个 。 在 下 图 中 你 可 以 看 到 两 个 工具 ，rviz 和 rqt_plot。 中 间 是 rqt_plot 的 截图 ， 你 可 以 
看 到 由 传感器 数据 绘制 的 曲线 。 另 外 两 个 截图 是 rviz; 截图 中 可 以 看 到 真实 机 器 人 的 三 维 显示 ，。 


ROS 是 一 个 使 用 BSD (Berkeley Software Distribution) 开源 协议 的 开源 软件 。 无 论 是 商业 应 用 还 是 科学 研究 它 都 是 免费 
的 。*-ros-pkg 包 受到 了 多 个 开源 协议 的 限制 。 


用 ROS 你 可 以 做 更 多 工作 。 你 可 以 使 用 库 中 的 代码 ， 改 进 后 再 次 共享 。 这 种 观念 丈 是 开源 软件 的 本 质 。 


ROS 已 经 友 布 了 多 个 版 本 ， 最 新 的 版 本 是 Indigo。 在 本 书 中 ， 我 们 使 用 的 版 本 是 Hydro， 因 为 这 个 版 本 更 加 稳定 ， 而 Indigo 
是 实验 版 本 ， 可 能 存在 错误 。 


下 面 会 介绍 如 何 安 六 Hydro 版 本 的 ROS。 即 使 在 本 书 中 我 们 使 用 Hydro， 但 是 在 实际 工作 中 ， 你 仍然 可 能 需要 安 濠 老 版 本 以 
便 运 行 一 些 代码 。 


如 前 所 述 ， 本 书 中 所 使 用 的 操作 系统 是 Ubuntu， 全 书 的 内 容 及 教程 将 以 该 系统 为 基础 。 如 果 你 习惯 使 用 其 他 操作 系统 又 想 
完成 本 书 的 学 习 ， 最 好 的 选择 就 是 安装 一 个 带 有 Ubuntu 的 虚拟 机 。 因 此 ， 本 章 末尾 会 介绍 虚拟 机 的 安装 方法 。 


当然 ， 如 果 你 想 在 其 他 系统 中 安 准 ROS， 你 可 以 根据 链接 http://wiki.ros.org/hydro/installation 中 的 指导 来 完成 。 


1.1 “PC 安 委 教程 


我 们 假设 你 已 经 安 六 了 Ubuntu12.04 系 统 。 我 们 使 用 这 个 版 本 的 Ubuntu， 因 为 它 是 一 个 长 期 支持 版 本 ， 配 备 了 Long-Term 
Support (LTS) 。 这 意味 着 社区 在 5 年 内 将 对 这 个 版 本 提供 维护 。 


此 外 ， 你 需要 具备 一 定 的 Linux 和 命令 工具 基本 知识 ， 例 如 终端 、vim、 创 建文 件 夹 等 。 如 果 需 要 学 习 这 些 工 具 ， 你 可 以 在 
网 上 找到 很 多 相关 的 资源 ， 或 者 你 也 可 以 参考 这 些 主 题 相关 的 图 书 。 


1.2 ”使 用 软件 库 安 遂 ROS Hydro 


2014 年 ，ROS 网 页 更 新 了 设计 风格 和 内 容 的 组 织 。 你 可 以 看 到 如 下 的 网 页 截图 : 
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在 菜单 中 ， 可 以 找到 关于 ROS 的 信息 以 及 ROS 是 否 适 用 于 你 的 系统 等 内 容 。 


ROS 的 安 闪 党 明 可 以 在 开始 (Getting Started) WDAZ (Install) 选项 卡 中 找到 。 
定义 安装 ; 在 这 种 情况 下 ， 你 可 能 


建议 你 在 系统 中 使 用 软件 库 而 不 是 源 代 码 安 疼 ROS， 除 非 你 是 一 个 专业 用 户 ， 想 进行 目 
喜欢 使 用 源 代码 安 妆 ROs。 
所 以 我 们 使 用 软件 库 安 丢 ROS， 下 面 将 开始 在 我 们 的 系统 中 配置 Ubuntu 软件 库 。 


1.3 如何 安 凌 VIrtualBox 和 Ubuntu 
完整 的 虚拟 机 ， 它 适用 于 x86 硬 件 ， 面 向 服务 器 、 人 台式 机 和 家 入 式 应 用 。VirtualBox 是 免费 的 ， 支 


VirtualBox 是 一 个 通用 、 
持 所 有 主流 的 操作 系统 。 几 乎 每 一 个 Linux 爱 好 者 都 会 使 用 它 。 
由 于 我 们 推荐 使 用 Ubuntu， 你 可 能 不 希望 更 改 计 算 机 现 有 的 操作 系统 。 而 如 VirtualBox 之 类 的 工具 就 可 以 满足 此 类 需求 。 


已 能 帮助 我 们 在 计算 机 上 虚拟 化 新 的 操作 系统 ， 而 无 需 对 计算 机 硬件 做 任何 改动 。 
后 面 的 章节 将 展示 如 何 安 妆 VirtualBox 和 Ubuntu。 此 外 ， 通 过 安 委 虚拟 机 ， 你 可 以 在 一 个 干净 的 操作 系统 中 完成 开 友 。 如 


果 你 过 到 任何 问题 ， 能 够 通过 快速 重启 计算 机 解决 ， 也 可 以 备份 虚拟 机 及 所 有 必要 的 机 器 人 安 沪 文件。 


1.4 在 BeagleBone Black 上 安装 ROS Hydro 


BeagleBone Black (BBB) 是 一 种 基于 ARM Cortex A8 处 理 器 的 低 成 本 开发 平台 。 此 开发 板 是 基于 Angstrom Linux 发 行 


版 制作 的 。 人 ngstr6m 由 一 支 希 望 统一 嵌入 式 系 统 Linux 发 行 版 的 小 型 团队 开发 ， 他 们 希望 操作 系统 是 稳定 且 用 户 友 好 的 。 


考虑 到 社区 的 开发 人 员 需 要 一 个 具有 一 些 通用 输入 /输出 (GPIO) 引 脚 的 机 载 计算 机 设备 ， 德 州 仪器 设计 了 BeagleBone 
Black。BeagleBone Black 平 台 是 BeagleBone 的 改进 版 。 开 发 板 的 主要 特性 包括 ARM Cortex A8 处 理 器 (时 钟 频率 为 1GHz， 
内 存 为 512MB) ， 具 有 以 大 网 、USB 接 口 、HDMI、46 引 脚 GPIO 接 口 。 这 些 GPIO 可 以 设置 为 数字 MO、ADC、 脉 宽 调 制 ， 以 及 
12C、9SPI 或 者 UART 等 通信 协议 接口 。GPIO 是 一 种 直接 将 传感器 和 执行 器 与 BeagleBone 连 接 的 简单 方法 。BeagleBone 如 下 图 
所 示 : 
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在 BeagleBone 开 发 板 刚 推出 时 ， 无 法 直接 在 Angstrom 发 行 版 上 安装 ROS。 由 于 这 个 原因 ， 通常 在 BeagleBone 上 安装 基 
于 Ubuntu 的 操作 系统 。 有 不 同 版 本 的 Ubuntu ARM 兼 容 BeagleBone Black 和 ROS， 推 荐 在 运行 ROS 的 平台 上 使 用 
Ubuntu13.04ARM raring armhf 的 镜像 。 


目前 已 有 了 Angstrom 发 行 版 的 ROS 版 本 安装 文件 。 安 装 步骤 可 以 参考 网 
址 http://wiki.ros.org/hydro/Installation/Angstrom。 除 此 之 外 ， 我 们 选择 在 Ubuntu ARM 上 安装 ROS 还 因为 这 个 友 行 版 更 常 
用 ， 此 外 它 还 可 以 用 于 其 他 基于 ARM 的 开发 板 ， 如 UDOO Odroid U3, Odroid X2 或 Gumstick。 


ARM 近 术 在 智能 手机 和 平板 计算 机 等 移动 设备 领域 迁 勃 友 展 。 除 了 增加 的 ARM cortex 运 算 性 能 ， 高 集成 度 和 低 功 耗 也 使 这 
项 扩 术 更 适合 于 目 主 机 器 人 系统 开 友 。 人 在 过 去 的 几 年 里 ， 开 上 友 人员 已 经 企 市 场 上 推出 多 款 ARM 平 台 。 其 中 一 些 特 性 类 似 于 
BeagleBone Black, Raspberry PI 或 Gumstick Overo。 上 此外， 更 强大 的 开发 板 (如 具备 双核 ARM Cortex A9 的 Gumstick 
DuoVero 或 四 核 版 Odroid U3, Odroid X2 或 UDOO) 也 已 经 上 市 。 


15 ”本章 小 结 


在 这 一 章 ， 我 们 学 习 了 如 何人 在 不 同 Ubuntu 设 备 上 (计算 机 、VirtualBox、BeagleBone Black) 安装 ROS Hydro。 通 过 这 
些 步 又， 你 已 经 在 系统 上 安装 了 一 切 必要 的 软件 ， 可 以 使 用 ROS 开 始 工作 ， 也 可 以 练习 本 书 中 的 示例 。 你 也 可 以 使 用 源 代 码 来 安 
和 妆 ROS。 但 这 样 做 需要 编译 所 有 代码 ， 因 此 只 适用 于 高 级 Linux 用 户 。 而 我 们 一 般 建议 你 使 用 软件 库 安 疼 ， 这 样 做 更 通用 ， 且 一 
般 不 会 出 现任 何 错误 或 问题 。 


如 果 你 对 Ubuntu 系 统 不 是 很 熟悉 ， 那 么 建立 一 个 虚拟 机 并 在 虚拟 机 上 学 习 使 用 ROS 会 更 加 方便 。 这 样 ， 如 果 你 在 安 闪 和 使 
用 过 程 中 友 生 任何 问题 ， 都 无 需 重 新 安 六 操作 系统 ， 只 需要 恢复 虚拟 机 镜像 文件 ， 然 后 束 可 以 重新 开始 。 


通常 情况 下 ， 虚 拟 机 不 能 访问 扩展 出 的 实际 硬件 ， 如 传感器 和 执行 器 。 尽 管 如 此 ， 你 仍 可 以 用 它 来 测试 算法 。 


下 一 草 将 学 习 ROS 的 染 构 、 一 些 重要 的 概念 ， 以 及 一 些 能 够 与 ROS 进 行 直接 交互 的 工具 。 


P2 ”ROS 系 统 染 构 及 概念 


一 旦 你 完成 了 ROs 系 统 的 安 丢 ， 你 肯定 会 想 “ 好 了 ， 我 已 经 安 丢 完 成 ， 那 么 下 一 步 要 做 什么 呢 ? ”在 本 章 我 们 将 学 习 ROS 系 
统 染 构 及 它 的 组 成 。 然 后 ， 我 们 会 开始 创建 节点 和 功能 包 ， 并 使 用 ROS 系 统 自 市 的 TurtleSim 示 例 。 


ROS 系 统 的 架构 主要 被 设计 和 划分 成 了 三 部 分 ， 每 一 部 分 都 代表 一 个 层级 的 概念 : 

文件 系统 级 (The Filesystem level) 

- 计算 图 级 (The Computation Graph level) 

: 开源 社区 级 (The Community level) 

第 一 级 是 文件 系统 级 。 在 这 一 级 ， 我 们 会 使 用 一 组 概念 来 解释 ROSs 的 内 部 构成 、 文 件 夹 结构 ， 以 及 工作 所 需 的 核心 文件 。 


第 二 级 是 计算 图 级 ， 体 现 的 是 进程 和 系统 之 间 的 通信 。 在 相 天 小 节 中 ， 我 们 将 学 习 ROSs 的 各 个 概念 和 功能 ， 包 括 建 立 系统 、 


处 理 各 类 进程 、 与 多 台 计 算 机 通信 等 。 


第 三 级 是 开源 社区 级 ， 我 们 将 解释 一 系列 的 工具 和 概念 ， 其 中 包括 在 开 友 人 员 之 间 如 何 共享 知识 、 算 法 和 代码 。 这 个 层级 非 
党 重要 ， 正 是 由 于 开源 社区 的 大 力 支持 ，ROS 系 统 才 得 以 快速 成 长 。 


2.1 -理解 ROS 文 件 系 统 级 


如 果 你 刚 接触 ROS， 无 论 是 准备 使 用 ROS 系 统 还 是 准备 开发 ROS 项 目 ， 你 都 会 觉得 ROS 中 的 各 种 概念 非常 奇怪 。 而 一 旦 你 驾 


SMA, BAK LMM TS o 


文件 系统 级 


功能 包 


mame] [We 


与 其 他 操作 系统 类 似 ， 一 个 ROS 程 序 的 不 同 组 件 要 被 放 在 不 同 的 文件 夹 下 。 这 些 文件 夹 是 根据 功能 的 不 同 来 对 文件 进行 组 织 
的 : 


功能 包 (Package) : 功能 包 是 ROS 中 软件 组 织 的 基本 形式 。 一 个 功能 包 具 有 用 于 创建 ROS 程 序 的 最 小 结构 和 最 少 内 容 。 它 
可 以 包含 ROS 运 行 的 进程 (节点) 、 配 置 文 件 等 。 


功能 包 清 单 (Package Manifest) : 功能 包 清 单 提供 关于 功能 包 、 许 可 信息 、 依 赖 关 系 、 编 译 标 志 等 的 信息 。 一 个 包 的 清单 
由 一 个 名 为 package.xml 的 文件 管理 。 


: 综合 功能 包 (Metapackage) : 如 果 你 将 几 个 具有 某 些 功能 的 功能 包 组 织 在 一 起 ， 那 么 你 将 会 获得 一 个 综合 功能 包 。 在 ROS 
Fuette 中 ， 这 种 包 的 组 织 形式 被 称 为 功能 包 集 (Stack) 。 为 了 保持 ROS 简 洁 ， 功 能 包 集 被 移 除 ， 现 在 使 用 综合 功能 包 实 现 这 个 功 
能 。 在 ROS 系 统 中 ， 存 在 大 量 不 同 用 途 的 综合 功能 包 ， 例 如 导航 功能 

. ZA 


综合 功能 包 清 单 (Metapackage manifest) : 综合 功能 包 清 单 (package.xml) 类 似 普通 功能 包 但 有 一 个 XML 格式 的 导出 标 
记 。 它 在 结构 上 也 有 一 定 的 限制 。 


: 消息 类 型 (Message (msg) type) : 消息 是 一 个 进程 发 送 到 其 他 进程 的 信息 。ROS 系 统 有 很 多 的 标准 类 型 消息 。 消 息 类 型 
的 说 明 存 储 在 my_package/msg/MyMessageType.msg 中 ， 也 就 是 对 应 功能 包 的 msg 文 件 夹 下 。 


- 服务 类 型 (Service (stv) type) : 服务 描述 说 明 存 储 在 my_package/srv/MyServiceType.strv 中 ， 定 义 了 在 ROS 中 由 每 个 进程 
提供 的 关于 服务 请 求 和 响应 的 数据 结构 。 


在 右 侧 的 截图 中 ， 可 以 看 到 turtlesim 功 能 包 的 内 容 。 你 看 到 的 是 一 系列 文件 和 文件 来， 包 仿 代码、 图片、 局 动 文 件 、 服 务 
和 消息 。 记 住 ， 截 图 显示 文件 的 一 个 简短 列表 ， 真 正 的 功能 包 会 更 多 。 
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上 -一 Pose.msg 
package. xml 
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2.2 ”理解 ROS 计 算 图 级 


ROS 会 创建 一 个 连接 到 所 有 进程 的 网 络 。 在 系统 中 的 任何 节点 都 可 以 访问 此 网 络 ， 并 通过 该 网 络 与 其 他 书 点 交互 ， 获 取 其 他 
节点 友 布 的 信息 ， 并 将 目 身 数据 友 布 到 网 络 上 。 


计算 图 级 


在 这 一 层级 中 最 基本 的 概念 包括 证 点 、 节 点 省 理 器 、 参 数 服 务 人 器、 消息、 服务、 主题 和 消息 记录 包 ， 这 些 概念 都 以 不 同 的 方 
式 辣 计算 图 级 提供 数据 : 


TA (Node) 节点 是 主要 的 计算 执行 进程 。 如 果 你 想 要 有 一 个 可 以 与 其 他 节点 进行 交互 的 进程 ， 那 么 你 需要 创建 一 个 
节点 ， 并 将 此 节点 连接 到 ROS 网 络 。 通 常情 况 下 ， 系 统 包含 能 够 实现 不 同 功 能 的 多 个 节点 。 你 最 好 让 每 一 个 节点 都 具有 特定 的 单 
一 的 功能 ， 而 不 是 在 系统 中 创建 一 个 包罗 万 象 的 大 节点 。 节 点 需要 使 用 如 foscpp 或 frospy 的 ROS 客 户 端 库 进 行 编写 。 


an 


节点 管理 器 (Master) ”节点 管理 器 用 于 节点 的 名 称 注 册 和 查找 等 。 它 也 设置 节点 间 的 通信 。 如 果 在 你 的 整个 ROS 系 统 中 
没有 节点 管理 器 ， 就 不 会 有 节 上 点、 服务、 消息 之 间 的 通信 。 需 要 注意 的 是 ， 由 于 ROS 本 身 就 是 一 个 分 布 式 网 络 系统 ， 你 可 以 在 某 


台 计 算 机 上 运行 节点 管理 器 ， 在 该 管理 器 或 其 他 计算 机 上 运行 节点 。 


- 人 参数 服务 器 (Parameter Server) ”参数 服务 器 能 够 使 数据 通过 关键 词 存储 在 一 个 系统 的 核心 位 置 。 通 过 使 用 参数 ， 就 能 够 
在 运行 时 配置 节点 或 改变 节点 的 工作 任务 。 


. 消息 (Message ) 节点 通过 消息 完成 彼此 的 沟通 。 消 息 包 含 一 个 节点 发 送 到 其 他 节点 的 信息 数据 。ROS 中 包含 很 多 种 标 
准 类 型 的 消息 ， 同 时 你 也 可 以 基于 标准 消息 开发 自 定义 类 型 的 消息 。 


主题 (Topic) 每 个 消息 都 必须 有 一 个 名 称 来 被 ROS 网 络 路 由 。 每 一 条 消息 都 要 发 布 到 相应 的 主题 。 当 一 个 节点 发 送 数 
据 时 ， 我 们 就 说 该 节点 正在 向 主题 发 布 消息 。 节 点 可 以 通过 订阅 某 个 主题 ， 接 收 来 自 其 他 节点 的 消息 。 一 个 节点 可 以 订阅 一 个 主 
题 ， 而 不 需要 该 节点 同时 发 布 该 主题 。 这 就 保证 了 消息 的 发 布 者 和 订阅 者 之 间 相 互 解 厢 ， 完 全 无 需 知 晓 对 方 的 存在 。 主 题 的 名 称 
必须 是 独一无二 的 ， 否 则 在 同名 主题 之 间 的 消息 路 由 就 会 发 生 错 误 。 


. 服务 (Service) 


时 ， 就 不 能 通过 主题 来 实现 了 。 在 这 种 情况 下 ， 服 


在 发 布 主 题 时 ， 正 在 发 送 的 数据 能 够 以 多 对 多 的 方式 交互 。 但 当 你 需要 从 某 个 节点 获得 一 个 请 求 或 应 答 
务 能 
一 个 节点 提供 某 个 服务 时 ， 所 有 的 节点 都 可 以 通过 使 用 ROS 客 户 端 库 编 写 的 代码 与 它 通 人 
消息 记录 包 (Bag) g, 
IRo FEA AAD 


aS 
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pp 


要 机 制 。 它 能 够 获取 并 记录 各 种 难以 收集 的 传 ; 
杂 机 


RA Z 
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-一 一 


你 可 以 看 到 节点 和 主题 


wo 


人 进行 实验 工作 时 ， 需 要 经 党 使 用 消息 记录 包 。 


够 允许 我 们 直接 与 菜 个 节点 进行 交互 。 此 外 ， 服 务必 须 有 唯一 的 名 称 。 当 
消息 记录 包 是 一 种 用 于 保存 和 回放 ROS 消 息 数据 的 文件 格式 。 

a 
ros-comm 库 中 实现 。 


以 及 哪些 石 点 订阅 哪些 主 


ER 
题 等 。 


我 们 可 以 通过 消息 记录 包 反 复 获 取 实 验 数 据 ， 进 行 必 要 的 开发 和 算法 测 


消息 记录 包 是 一 种 用 于 存储 数据 的 重 
tA 
在 下 图 中 你 可 以 看 到 计算 图 级 的 图 形 化 表示 (PARSE) 。 它 表示 了 真实 机 器 人 在 真实 条 件 下 系统 的 工作 状态 。 在 图 中 ， 
容 需 要 使 用 其 他 工具 进行 图 形 化 展示 。 用 于 创建 该 图 表 的 工具 是 rqt_graph， 在 第 3 章 中 将 学 习 a 到 更 多 相关 知识 。 这 些 概念 在 
CGoint_state_publisher > /joint_ states 


此 节点 状态 图 中 并 没有 消息 、 消 息 记 录 包 、 参 数 服务 器 和 服务 。 这 些 内 
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2.3 ”理解 ROS 开 源 社 区 级 


- 反 行 版 (Distribution) 


A+ 


挥 类 似 的 作用 。 这 使 得 ROS 软 件 安 装 更 加 容易 ， 而 且 能 够 通过 一 个 软件 集合 维持 一 致 的 版 本 。 
机 器 人 软件 与 程序 。 


ARN 


as) 


ROS 开 源 社区 级 的 概念 主要 是 ROS 资 源 ， 其 能 够 通过 独立 的 网 络 社 区 分 享 软件 和 知识 。 这 些 资源 包括 : 
ROS 发 行 版 是 可 以 独立 安 
- 软件 库 (Repository) 


- ROSH (ROS Wiki) 


件 、 提 供 更 正 或 更 新 、 编 写 教 程 以 及 其 他 行为 。 


=A 
oa) 


NSS 


带 有 版 本 号 的 一 系列 综合 功能 包 。ROS 发 行 版 像 Linux 发 行 版 一 样 发 


ROS 依 赖 于 共享 开源 代码 与 软件 库 的 网 站 或 主机 服务 ， 在 这 里 不 同 的 机 构 能 够 发 布 和 分 享 各 自 的 
ROS Wiki 是 用 于 记录 有 关 ROS 系 统 人 


,的 主要 论坛 。 任 何人 都 可 以 注册 账户 、 贡 献 自 己 的 文 


- Bug 提交 系统 (Bug Ticket System) 如 果 你 发 现 问题 或 者 想 提 出 一 个 新 功能 ，ROS 提 供 这 个 资源 去 做 这 些 。 


- 邮件 列表 (Mailinglist) ROS 用 户 邮 件 列表 是 关于 ROS 的 主要 交流 渠道 ， 能 够 像 论坛 一 样 交流 从 ROS 软 件 更 新 到 ROS 软 
件 使 用 中 的 各 种 疑问 或 信息 。 


- ROS 问 答 (ROS Answer) 用 户 可 以 使 用 这 个 资源 去 提问 题 。 


. 博客 (Blog) 你 可 以 看 到 定期 更 新 、 上 照片 和 新 闻 ， 网 址 是 http://www.ros.org/news。 


24 ” ROS 系统 试用 练习 


现在 是 时 候 对 之 前 学 习 的 内 容 进 行 一 些 练 习 了 。 在 下 面 的 几 小 节 中 ， 你 将 看 到 练习 包 的 创建 ， 使 用 节点 、 参 数 服 务 器 以 及 通 
过 Turtlesim 移 动 仿 真 机 器 人 。 


25 ABs 


本 章 介绍 了 ROSs 系 统 的 碎 构 及 其 工作 方式 的 基本 信息 。 学 习 了 一 些 基 本 概念 、 工 具 及 如 何 同 节点 、 主 题 和 服务 进行 交互 的 示 
例 。 一 开始 ， 所 有 这 些 概念 可 能 看 起 来 有 些 复杂 上 且 不 太 实 用 ， 但 在 后 面 的 章节 中 ， 你 会 逐渐 理解 这 样 的 应 用 。 


各 位 读者 最 好 在 继续 后 续 章 节 的 学 习 之 前 ， 对 这 些 概 念 及 示例 进行 练习 ， 因 为 在 后 面 的 章节 里 ， 我 们 将 假定 你 已 经 熟悉 所 有 
的 概念 及 其 用 途 。 


请 注意 如 果 想 查询 某 个 名 词 或 功能 的 解释 ， 且 无 法 在 这 本 书 中 找到 相关 内 容 或 答案 ， 那 么 可 以 通过 以 下 链接 访问 ROS 官 方 吹 
源 http://www.ros.org。 而 且 你 还 可 以 通过 访问 ROS 社 区 http://answers.ros.org 提 出 自己 的 问题 。 


在 下 一 章 中 ， 将 学 习 如 何 使 用 ROS 工 具 调 试 和 可 视 化 数据 。 这 些 将 帮助 你 友 现 软件 运行 的 问题 ， 并 且 指 导 你 对 它 的 运行 进行 
调整 。 


Bim “可视化 和 调试 工具 


ROS 附 市 了 大 量 功能 强大 的 工具 ， 帮 助 用 尸 和 开 友 人 员 可 视 化 和 调试 代码 ， 以 便 检测 并 解决 软 硬 件 问题 。 其 中 包括 消息 日 志 
系统 (类似 log4cxx) 、 诊 断 消息 、 可 视 化 以 及 检测 工具 。 这 些 工具 展示 了 哪些 书 点 正在 运行 和 它们 是 如 何 连 接 的 。 
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本 草 我 们 还 会 展示 如 何 用 GDB 调 试 器 调试 ROS 节 上 后， 介绍 用 于 日 志 记 录 的 APl， 以 及 如 何 设 置 日 志 记 录 级 别 。 接 着 ,我 们 将 
解释 如 何 用 ROS 工 具 集 检测 哪些 进程 正在 运行 以 及 它们 之 间 通 信 的 内 容 。 例 如 ， 在 下 图 所 示 的 系统 可 视 化 图 中 可 以 看 到 正在 运行 
的 节操 以 及 用 连 线 表示 的 数据 流 。 这 个 工具 是 rqt_graph， 这 里 显示 的 是 REEM 机 器 人 在 Gazebo 仿 真 中 运行 的 节点 和 主题 。 


从 图 中 我 们 可 以 看 到 多 个 天 于 机 械 臂 、 胶 体 和 头 部 的 控制 器 、Movelt! 的 move_group 节 点 、 抓 取 和 放置 操作 服务 以 及 
play motion 节 点 的 预存 储 动作 。 其 他 节点 发 布 joint_states、 大 量 机 器 人 控制 器 以 及 移动 底盘 的 手柄 控制 等 信息 。 


同样 ， 本 章 会 介绍 标量 数据 的 时 序 绘 图 工具 ， 视 频 流 数据 的 可 视 化 图 ， 以 及 用 于 不 同类 型 数据 的 3D 可 视 化 工具 rviz (或 
rqt_rviz) 等 ， 如 下 图 所 示 : 
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可 以 使 用 下 面 命 令 运行 上 图 的 REEM 机 器 人 仿真 : 


$ roslaunch reem 2dnav gazebo reem navigation.launch 


注意 ， 在 开始 安装 此 仿真 前 ， 请 认真 阅读 下 面 网 页 的 使 用 说 明 : http://wiki.ros.org/Robots/REEM, 
在 本 节 中 ， 将 介绍 以 下 内 容 : 
- 如 何在 ROS 中 调试 和 优化 代码 。 


. 介绍 在 代码 中 添加 消息 日 志 并 设置 不 同 的 级 别 、 命 名 、 特 定 条 件 和 流 选项 。 这 里 我 们 将 解释 trqt_logeef level 和 ftqt_console 接 


口 ， 它 们 可 以 分 别 设置 节点 错误 级 别 和 消息 可 视 化 。 


介绍 如 何 通 过 列表 来 检测 ROS 系 统 状 态 ， 包 括 运 行 的 节点 、 主 题 、 服 务 和 它们 之 间 传 递 信息 的 行为 以 及 ROS 节 点 管理 器 中 
y- 
P 


声明 的 参数 等 。 我 们 将 介绍 以 有 向 图 形式 显示 主题 和 节点 的 tqt_graph， 和 可 以 用 来 修改 动态 参数 的 rqt_reconfigure。 
- 讲解 如 何 使 用 runtime_monitor 和 robot_monitor 接 口 可 视 化 诊断 信息 。 


- 讲解 如 何 使 用 rqt_plot 绘 制 特定 消息 的 标量 数据 。 对 于 非 标量 数据 ， 我 们 将 讲解 ROS 中 的 其 他 rqt 工 具 ， 例 如 用 


fqt_imapge_view 显 示 图 像 以 及 用 rqt_tviz 以 3D 形 式 显 示 多 维 数据 。 还 包括 如 何 可 视 化 标记 和 交互 式 标 记 。 


` 介绍 坐标 系 以 及 如 何 将 它们 集成 到 ROS 消 息 和 可 视 化 工具 之 中 。 以 及 如 何 使 用 trqt_tf tree 来 可 视 化 转换 坐标 系 树 Transfotm 


Frame (tf) o 
` 讲解 如 何 保存 主题 发 送 的 消息 ， 以 及 如 何 重 播 它们 用 于 仿真 或 测试 目的 ， 并 介绍 rqt_bag 接 口 。 
. 最 后 ， 将 介绍 trqt_pui 接 口 ， 以 及 如 何在 一 个 GUI 窗口 中 排列 显示 它们 。 


大 部 分 rqt 工 具 可 以 在 终端 输入 名 称 运行 ， 例 如 rqt_console， 但 有 时 不 行 ， 必 须 使 用 rosrun rqt_reconfigure 
rqt_reconfigure， 注 意 名 字 里 然 是 重复 的 ， 但 其 实 前 一 个 是 功能 包 的 名 称 ， 后 一 个 是 节点 名 称 。 


3.1 调试 ROS 节 点 


ROS 忆 点 可 以 像 正 常 程序 一 样 调试 。 调 试 程序 在 系统 中 运行 时 有 一 个 进程 号 (PID) 。 你 可 以 用 任何 标准 工具 (如 gdb) # 
行 调试 。 同 样 可 以 用 valgrind 检 查 内 存 泄漏 ， 或 者 用 callgrind 分 析 算 法 性 能 。 请 记 住 使 用 下 面 的 命令 运行 一 个 节点 : 


$ rosrun chapter3 tutorials examplel 
很 遗憾 ， 你 不 能 通过 下 面 的 方式 局 动 命令 : 


$ gdb rosrun chapter3 tutorials examplel 


在 接 下 来 的 部 分 中 ， 我 们 将 解释 如 何 调用 这 些 工具 调试 ROS 忆 点， 以 及 如 何在 代码 中 添加 日 志 消 息 ， 让 问题 诊断 更 简单 。 这 
样 即使 没有 二 进 制 调 试 文件 ， 也 可 以 诊断 基本 问题 。 然 后 ， 我 们 将 讨论 ROS 目 检 工 具 ， 测 试 书 点 间 损 坏 的 连接 。 因 此 ， 这 草 的 概 
述 是 目下 而 上 ， 实 际 的 问题 诊断 方式 是 自 上 向 下 。 


3.2 日 志 信 息 


通过 信息 记录 显示 程序 的 运行 状态 是 好 的 习惯 ,但 需要 确定 这 样 做 不 会 影响 软件 的 运行 效率 和 输出 的 清晰 度 。 在 ROS 中 有 满 
足以 上 要 求 并 且 内 置 于 log4cxx (众所周知 的 log 有 记 录 库 的 一 个 痛 口 ) 之 上 的 API。 简 单 地 说 ， 我 们 有 不 同 层级 的 调试 信息 输 
出 ， 每 条 信息 都 有 上 自己 的 名 称 ， 并 根据 相应 条 件 输出 消息 。 如 果 它 们 被 当前 见长 级 别 掩 匡 (甚至 在 编译 时 ) ， 它 们 对 性 能 没有 影 
响 。 它 们 与 ROS 其 他 工具 完全 集成 来 可 视 化 或 过 滤 来 自 所 有 运行 节点 的 消息 。 


3.3 ”检测 系统 状态 


当 系 统 运 行 时 ， 可 能 有 数 个 书 点 和 数 十 个 主题 在 节点 中 友 布 消息 。 同 时 ， 有 些 证 点 可 能 也 会 提供 行为 或 服务 。 对 于 大 型 系统 
来 说 ， 通 过 提供 一 些 工具 让 我 们 看 到 系统 在 给 定时 间 的 运行 状态 是 非常 重要 的 。ROS 对 此 提供 了 一 些 基本 但 非常 强大 的 工具 ,， 包 
括 从 CLI 到 GUI 应 用 。 


34 ”设置 动态 参数 


如 果 一 个 节操 配置 了 一 个 动态 重 配置 参数 服务 器 ， 在 工作 中 束 可 以 使 用 rqt_reconfigure 进 行 修改 。 运 行 下 面 的 代码 局 动 一 
个 市 有 几 个 参数 动态 重 配置 的 服务 器 ( 见 功能 包 cfg 文 件 夹 中 的 cfg 文 件 ) : 


$ roslaunch chapter3 tutorials example6.launch 
使 用 下 面 命令 启动 动态 重 配置 服务 器 ， 打 开 GUI: 


$ rosrun rqt reconfigure rqt reconfigure 


在 左边 的 列表 中 选择 example6 服 务 器 ， 然 后 残 能 看 到 它 的 参数 ， 并 可 以 直接 修改 。 运 行 源 代 码 中 一 个 回调 方法 的 代码 会 对 
值 的 有 效 性 进行 检查 。 也 束 是 当 回 调 方 法 执行 时 ， 在 示例 中 这 些 参数 的 更 改 将 立即 生效 。 所 述 内 容 如 下 图 所 示 : 


0 |: 
0 | 


动态 参数 原本 是 为 驱动 程序 设计 的 ， 这 使 参数 修改 变 得 简单 。 因 此 动态 参数 已 经 在 一 些 驱 动 上 得 以 应 用 。 尽 管 如 此 ， 它 们 也 
可 以 用 于 任何 其 他 节点 。 驱 动 程序 实现 的 示例 ， 如 Hokuyo 激 光 测 距 仪 的 hokuyo_node 驱 动 或 FireWire camera1394 驱 动 。 
FireWire 报 像 头 采用 通用 的 驱动 程序 以 支持 传感器 一 些 配 置 参 数 的 改变 ， 如 帧 速率 、 快 门 速度 和 亮度 等 。 可 以 运行 下 面 的 命令 启 
动 FireWire ROS 报 像 头 驱动 (IEEE1394,，a 和 b) : 


$ rosrun cameral394 cameral394 node 


当 报 像 头 运行 时 ， 我 们 可 以 用 rqt_reconfigure 配 置 参 数 ， 将 会 看 到 类 似 下 图 的 界面 : 
File 

guid: |08144361026320a0 

video mode: 


Formato Mode5 ('640x480 mono8’') 


frame id: 


frame rate: 
iso speed: 
camera Info_ url: 


bayer pattem: 


bayer method: |image proc (") 


一 — i i 一 | b o | 一 -| 二 | SO 一 -一 一 画面 i 


aurozpngnmess: Auto (2) —— =< =< =< 
brightness: - 
auto_exposure: |; 
exposure: 
auto_gain: | 

gain: 
auto_gamma: 


gamma: 
auto_hue: |! 


ma a 
a 


hue: 


auto iris: |I 


iris: 0 


auto_saturation: | 


saturation: 

auto sharpness: |! 
sharpness: » 
auto_shutter: |1 
shutter: í 


auto_white_balance: 
white balance BU: 
white balance RV: 


请 注意 ， 我 们 将 在 第 5 章 介绍 如 何 使 用 摄像 头 ， 我 们 还 将 从 开发 人 员 的 角度 解释 这 些 参数 . 


ROS 还 提供 了 另外 一 些 工具 来 检测 给 定 功能 包 中 所 有 组 件 的 潜在 问题 。 使 用 roscd 移 动 到 你 想 要 分 析 的 功能 包 路 径 下 ， 然 后 
运行 roswtf。 对 于 chapter3_tutorials， 我 们 可 以 获得 以 下 输出 。 请 注意 ， 如 果 你 运行 了 一 些 代码 ， 融 可 以 用 ROS 图 进行 分 析 。 


运行 roslaunch chapter3 tutorials example6.launch 命 令 ， 输 出 如 图 所 示 。 
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process(rosout-il: started with pid [12411] 


started core service |/rosout! 
processlexampleo-7): started with pid [124735] 


| INFO] (1404156269. 276210812): New configuration received with Level 


通 单 情况 下 ， 我 们 和 希望 获得 的 结果 没有 任何 错误 和 和 警告， 但 是 很 多 错误 和 警告 在 系统 正 弟 运 行 时 是 没有 影响 的 。 在 上 图 
中 ，roswtf 没 有 检测 到 任何 锋 误 ， 只 有 一 个 关于 pip 的 警告 ， 这 有 时 可 能 是 由 系统 安 妆 的 Python 代码 产生 的 问题 。 请 ; 
意 ，roswtf 的 作用 是 检测 信号 的 潜在 问题 ， 然 后 融 像 上 面 示 例 一 样 ， 我 们 再 来 检查 这 些 提 示 是 否 有 意义 。 


另 一 个 有 用 的 工具 是 catkin_lint， 可 以 帮助 catkin 诊 断 错误 ,通常 在 CMakeLists.txt 和 package.xml 文 件 中 。 在 
chapter3 tutorials 中 ， 得 到 如 下 输出 : 


$ catkin lint -W2 --pkg chapter3 tutorials 


使 用 参数 -W2， 束 看 到 通常 会 被 忽略 的 警告 ， 如 下 图 所 示 : 
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> jigits and T aii $< P 
process [rosout=1]: started nu pid [12411] 


ROS 节 点 可 以 使 用 diagnostics (诊断 ) 主题 提供 诊断 信息 ， 并 为 此 提供 了 一 个 API 用 于 以 标准 方式 上 友 布 诊断 信息 。 信 息 遵 循 
diagnostic_msgs/DiagnositcStatus 的 消息 类 型 ， 它 允许 我 们 指定 一 个 级 别 (24. eR) 、 名 称 、 消 息 和 硬件 ID 以 及 
diagnostic msgs/KeyValue 列 表 ， 对 应 键 (key) 和 值 (value) 字符 串 。 


有 趣 的 部 分 是 收集 诊断 信息 并 将 其 可 视 化 的 工具 。 在 最 基本 的 层面 上 ，rqt_runtime_monitor 通 过 diagnostics 主 题 直 接 友 
布 可 视 化 信息 。 运 行 example7 节 点 ， 它 通过 诊断 主题 发 布 信息 并 可 以 通过 可 视 化 工具 查看 诊断 信息 : 


$ roslaunch chapter3 tutorials example7.launch 


$ rosrun rqt runtime monitor rqt runtime monitor 


前 面 的 命令 输出 如 下 所 示 : 


(«)Runtime Monitor 
:… @ Stale (0) 
+ @ Errors (0) 
: AN Warnings (0) 
EG} & Ok (1) 
“ example7: status: ok 


当 系 统 很 大 时 ， 我 们 可 以 使 用 diagnostic aggregator 汇 总 诊断 信息 。 在 diagnostics_agg 里 处 理 和 归 类 diagnostics 主 题 的 
消息 并 重新 发 布 。 这 些 汇总 诊断 消息 通过 rqt_robot_ monitor 实 现 可 视 化 。 诊 断 汇 总 器 通过 一 个 配置 文件 进行 配置 ， 例 如 下 面 的 
这 一 个 (参看 chapter3 tutorials 中 的 config/diagnostic aggregator.yaml 文 件 ) ， 并 使 用 AnalyzerGroup 定 义 不 同 的 
analyzers (分 析 器 ) : 


type: AnalyzerGroup 

path: Sensors 

analyzers: 

status: 

type: GenericAnalyzer 
path: Status 
startswith: example7 
num items: 1 


launch 文 件 已 经 在 之 前 的 代码 中 使 用 ， 并 以 之 前 的 配置 运行 诊断 节点 aggregator node, WF: 


$ rosrun rqt robot monitor rqt robot monitor 


现在 ， 我 们 可 以 比较 rqt_ runtime monitor 与 rqt robot monitor 的 可 视 化 ， 如 下 图 所 示 。 


= @& (Err: 2, Wen: 0) Sensors Error Error 
口 Error 
© example?: status error 


3.7 ”绘制 标量 数据 图 


我 们 可 以 使 用 ROS 中 现 有 的 一 些 通 用 工具 轻松 地 绘制 标量 数据 图 。 当 然 非 标 量 数 据 也 可 以 绘制 ， 但 是 要 分 别 在 不 同 的 标量 域 
里 进行 。 我 们 之 所 以 在 此 仅 讨 论 标 量 数 据 ， 是 因为 对 于 大 多 数 非 标量 数据 ， 有 专门 的 工具 能 够 更 好 地 对 其 进行 表示 ， 我 们 会 在 后 
面 进行 部 分 介绍 ， 例 如 图 形 、 位 姿 、 方 向 和 角度 等 。 

用 rqt_plot 画 出 时 间 趋 势 曲线 


在 ROS 系 统 中 ， 标 量 数 据 可 以 根据 消息 中 提供 的 时 间 截 作为 时 间 序 列 绘 制 。 然 后 ， 我 们 束 能 够 在 y 轴 上 使 用 rqt_plot 工 具 绘 
制 标量 数据 。rqt_plot 工 具有 一 套 功 能 强大 的 参数 语法 ， 人 允许 我 们 在 结构 化 消息 中 指定 多 个 字段 (当然 使 用 了 相当 简明 的 方 


式 ) ， 也 可 以 在 GUI 中 手动 添加 或 删除 主题 和 字段 。 


为 了 能 够 实际 展示 rqt_plot 工 具 ， 我 们 使 用 example4 市 点 ， 它 在 两 个 不 同 的 主题 中 分 别 友 布 一 个 标量 和 一 个 矢量 ( 非 标 
量 ) ， 这 两 个 主题 分 别 是 温度 (temp) 和 加 速度 (accel) 。 在 这 些 消息 中 的 值 是 随机 生成 的 ， 所 以 它们 没有 实际 意义 ， 仪 用 于 


示范 曲线 绘制 。 那 么 首先 以 下 面 的 命令 运行 节点 : 


$ rosrun chapter3 tutorials example4 


为 了 能 够 画 出 消息 ， 我 们 必须 知道 具体 的 格式 ; 如果 你 不 知道 具体 格式 则 使 用 rosmg show<msg type> 获 取 。 对 于 标量 数 
据 ， 我 们 通常 使 用 data 作 为 字段 名 来 表示 实际 的 值 。 因 此 ， 对 于 temp 主 题 ， 数 据 格式 为 Int32， 运 行 下 面 命令 : 


$ rosrun rqt plot rqt plot /temp/data 


RETREAT, RlmealHZeewARS Bet. Wrage. 


在 示例 万 点 提供 的 accel 主 题 里 ， 我 们 看 到 一 个 Vector3 的 消息 (你 可 以 通过 rostopic type/accel 来 查看 ) ， 我 们 可 以 在 一 
个 plot 图 中 分 别 绘制 三 个 字段 的 曲线 。Vector3 消 息 包 合 三 个 字段 x、y 和 z。 我 们 可 以 使 用 逗号 (，) 来 区 分 字段 或 者 像 下 面 一 
样 使 用 更 加 简洁 的 方式 : 


$ rosrun rqt plot rqt plot /accel/x:y:z 


绘制 的 曲线 图 如 下 : 


我 们 还 可 以 将 每 个 字段 分 开 绘制 。 由 于 rqt_plot 不 直接 支持 这 个 功能 ， 因 此 我 们 需要 使 用 rqt_gui 将 三 个 图 表 手 动 分 开 ， 如 下 
图 所 示 : 


$ rosrun rqt gui rqt gui 


rqat_plothGUIZ ERRE. FEROS hydrozai, RAmatplotlibs7s. WE, RATTLERS RQ, MIA 
支持 更 多 的 时 间 序 列 。 可 以 点 击 配置 (configuration) 按钮 选择 (如 右 图 所 示 ) : 


cok] cancel 


3.8 图 像 可 钢化 


在 ROS 系 统 中 ， 我 们 可 以 创建 一 个 节点 ， 在 节点 中 展示 来 自 即 插 即 用 摄像 头 的 图 像 。 这 是 一 个 复杂 数据 主题 的 例子 ， 这 些 数 
据 可 以 使 用 特殊 工具 更 好 地 可 视 化 或 分 析 。 你 只 需要 一 个 摄像 头 来 完成 这 些 ， 例 如 你 笔记 本 上 的 webcam。 在 example8 节 点 
中 ， 通 过 调用 OpenCV 库 实现 一 段 基本 的 摄像 头 捕捉 程序 ， 然 后 在 ROS 中 将 采集 到 的 cv::Mat 图 像 转换 到 ROS 图 像 ， 这 样 就 可 以 
在 主题 中 友 布 了 。 这 个 节点 会 在 /camera 主 题 里 太 布 图 像 帧 。 


我 们 仅 会 使 用 launch 文 件 运行 节点 并 进行 图 像 捕 捉 和 发 布 工作 。 节 点 中 的 代码 对 于 读者 来 说 可 能 很 陌生， 但 是 在 下 面 的 章 
节 中 ， 我 们 将 会 介绍 如 何在 ROSs 中 使 用 摄像 头 和 图 像 ， 到 时 候 再 回来 看 这 些 代 码 ， 你 就 会 完全 理解 节点 的 工作 原理 和 每 行 代码 的 
SX: 


$ roslaunch chapter3 tutorials example8s.launch 


一 旦 节点 运行 起 来 ， 我 们 就 能 够 列 出 主题 列表 (rostopic list) ， 并 查看 是 否 包 含 /camera 主 题 。 查 看 是 人 否 正 确 捕捉 到 图 像 
有 一 个 简单 直接 的 方法 ， 使 用 rostopic hz/camera 语 句 查看 在 主题 中 收 到 的 图 像 更 新 频率 是 多 少 。 频 率 通常 是 30Hz， 如 下 图 所 


省 \. 


enrique@pc-acer[hydro] :svn: 
~/svn/ros book hydro ws/src/ros book v2/chapter3 tutorials$ rostopic hz /camera 
subscribed to [/camera] 
average rate: 10.728 
min: 0.084s max: 0.099s std dev: 0.00474s window: 
average rate: 10.746 
min: ©.084s max: 0.099s std dev: ©.00441s window: 


average rate: 10.725 


min: 0.084s max: 0.099s std dev: 0.00426s window: 
average rate: 10.710 

min: 0.084s max: 0.100s std dev: 0.00409s window: 
average rate: 10.702 
i min: 0.084s max: 0.100s std dev: 0.00398s window: 


显示 早 一 图 片 


想 要 查看 一 张 图 像 ， 我 们 不 能 直接 使 用 rostopic echo/camera 命 令 ， 因 为 这 会 使 用 文本 格式 输出 图 片 的 信息 ， 数 据 量 会 非 
党 巨大 ， 不 可 能 进行 任何 有 效 的 分 析 。 因 此 ， 我 们 会 调用 下 面 的 命令 : 


$ rosrun image view image view image:=/camera 


这 里 使 用 了 image view 节 点 ， 在 窗口 中 展示 了 给 定 主 题 (使 用 image 参 数 ) WER, NEP. MRI PR Bee 
地 显示 在 主题 内 发 布 的 每 一 幅 图 像 或 帧 ， 即 使 数据 来 自 网 络 也 可 以 实现 。 你 可 以 通过 在 窗口 中 点 击 右边 的 按钮 将 当前 帧 保存 在 硬 
盘 里 ， 通 常会 存在 home 目 录 下 或 者 ~/.ros 目 录 下 。 


ROS hydro 还 有 rqt_image_view， 支 持 在 一 个 窗口 查看 多 个 图 像 ， 但 不 允许 点 击 右边 按钮 保存 图 像 。 我 们 可 以 在 GUI 上 手 
动 选择 图 像 主题 或 使 用 mage view: 


$ rosrun rqt image view rqt image view 


前 面 命令 结果 如 右 图 所 示 : 


国 Image View 硬 @ -o 


|/camera a | (a) M [49.01m 4 
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ROS 提 供 了 一 个 基于 OpenCV 校 准 API 的 摄像 头 校准 界面 。 我 们 将 在 第 5 草 介 绍 如 何 使 用 摄像 头 ， 包 括 单 上 自 和 双 目 摄像 头 以 
及 ROSs 图 像 管道 (image_proc 和 stereo_image_proc) 相 天 内 容 ， 这 些 能 修正 摄像 头 图 像 失真 、 计 算 双 目 摄 像 头 深度 差异 ， 并 


3.9 3D 可 催化 


正如 我 们 在 前 面 章 节 中 所 见 ， 有 很 多 设备 (例如 双 目 摄像 头 、3D 激 光 和 Kinect 传 感 器 等 ) 能 够 提供 3D 数 据 。 它 们 通常 使 用 
FACTS. 〈 组 织 好 或 未 组 织 的 ) 。 基 于 上 述 原因 ， 能 够 使 用 工具 实现 3D 数 据 可 视 化 融 非 澡 有 用 了 。 本 节 我 们 将 介绍 ROSS 系统 中 
的 rviz 或 rqt_rviz 工 具 。 它 集成 了 能 够 完成 3D 数 据 处 理 的 OpenGL 接 口 ， 能 够 将 传感器 数据 在 模型 化 世界 (world) 中 展示 ， 过 程 
是 先 使 用 传感器 坐标 系 读 取 测 量 值 ， 再 将 这 些 读 数 按照 之 间 的 相对 位 置 在 正确 的 位 置 绘制 。 


3.10 PYF SEINE 


Roth, SRA es ARS, RES A, ARERR TAG. RAAT ESTES 
了 大 量 的 时 间 ， 或 是 实验 难以 重 现 等 诸如 此 类 的 原因 导致 实验 不 能 顺利 完成 。 基 于 以 上 原因 ， 将 实验 数据 记录 下 来 用 于 未 来 的 分 
析 、 处 理 、 开 发 和 算法 验证 就 是 一 个 很 好 的 实践 方法 。 然 而 要 保证 存储 数据 正确 并 能 够 用 于 离线 回放 却 不 是 容易 的 事情 ， 因 为 很 
多 实验 并 不 一 定 能 反复 进行 。 幸 运 的 是 ，ROS 中 的 强大 工具 使 我 们 能 够 完成 这 项 任务 。 

ROS 能 够 存储 所 有 节操 通过 主题 友 布 的 消息 。 它 能 够 创建 一 个 消息 记录 包 文 件 (bag) 来 保 丰 消息 ， 并 包谷 消息 的 所 有 字段 
参数 和 时 间 稚 。 这 人 允许 离线 回放 实验 并 模拟 真实 的 状态 ， 包 括 消息 的 时 | 间 延 迟 。 甚 至 ROS 工 具 能 够 非常 有 效 地 在 处 理 高 速 数据 流 
时 以 充足 的 数据 组 织 方式 来 实现 这 一 切 。 

在 下 面 的 小 忆 中 ， 我 们 会 解释 ROS 中 提供 的 这 些 用 于 存储 和 回放 消息 记录 包 文 件 的 工具 ， 消 息 记录 包 文 件 由 ROS 开 友 者 设 
计 ， 使 用 二 进 制 格式 存储 数据 。 我 们 还 将 看 到 如 何 管理 这 些 文件 ， 即 检查 文件 内 容 (消息 的 数量 、 主 题 等 ) 、 文 件 压缩 、 分 割 和 
合并 多 个 记录 包 文 件 。 


3.11 ”应 用 rqt 与 rqt_gui 揪 件 


从 ROS Fuerte 发 布 以 来 ，rx 应 用 或 工具 已 经 被 rqt 节 点 替代 。 它 们 基本 上 是 一 样 的， 只 有 一 小 部 分 的 升级 、bug 修 正和 新 功 
能 。 我 们 使 用 下 面 列表 对 本 草 中 介绍 的 工具 进行 说 明 (ROS hydro 的 rqt 工 具 以 及 对 应 被 蔡 代 的 之 前 的 ROS 发 布 版 本 ) : 


ROS hydro rqt 工具 替代 (ROS fuerte 或 之 前 版 本 ) 
rqt console #llrqt logger level rxconsole 
rat graph rxgraph 
rat reconfigure rqt reconfigure dynamic reconfigure reconfigure gui 
rqt plot rxplot 
rqt image view image view 
rqt bag rxbag 


ZEROS hydro 中 ， 这 些 可 以 单独 运行 的 插件 甚至 更 多 ， 能 够 作为 命令 行 (rqt_shell) 、 主 题 的 发 布 者 (rqt_publisher) 、 
消息 类 型 的 检查 者 (rqt_msg) 等 ， 本 章 内 容 湖 盖 了 这 些 重要 的 插件 。 甚 至 rviz 都 有 一 个 名 为 rqt_rviz 的 插件 ， 还 能 够 集成 在 最 新 
的 rqt_gui 界 面 中 。 我 们 可 以 运行 这 个 GUI 并 在 窗口 上 手动 添加 和 排列 一 些 插件 ， 如 本 章 之 前 给 出 的 一 些 示例 : 


$ rosrun rqt gui rqt gui 


3.12 Ae 


TEARS ABST SOON, FME SRV BARRMBRE, tailed, SRIMUSABLALA. AL 
通过 这 些 工 具 评估 或 验证 设计 质量 。 作 为 机 器 人 开 友 人 员 ， 你 将 在 开 及 过程 中 逐渐 深入 体验 这 些 特殊 的 概念 和 工具 。 


现在 你 知道 了 如 何在 代码 中 包含 不 同 级 别 不 同 详细 程度 的 日 志 信 息 。 这 能 够 帮助 你 调试 节点 错误 。 为 了 实现 这 个 目的 ， 你 还 
需要 使 用 其 他 一 些 ROS 中 包含 的 强大 工具 ， 例 如 rqt_console 界 面 。 另 外 ， 你 也 可 以 检查 或 询 出 运行 中 的 节点 、 友 布 的 主题 和 系 


统 运行 时 提供 的 服务 。 这 包含 了 使 用 rqt_graph 工 具 检 测 节点 状态 图 。 


对 于 可 视 化 工具 ， 你 已 学 习 了 使 用 rqt_plot 绘 制 标量 数据 曲线 。 它 能 够 以 更 加 直观 的 特定 变量 分 析 方 式 友 布 你 的 节点 。 类 似 
地 ， 你 能 够 查看 更 加 复杂 的 数据 类 型 〈 非 标量 数据 ) 。 这 包括 使 用 rqt image _view 和 rqt_rviz 理 看 图 像 和 3D 数 据 ， 也 可 以 使 用 
了 这些 工具 校正 和 调整 摄像 头 图 像 。 


最 后 ， 你 已 经 通过 学 习 rosbag 工 具 学 会 了 记录 和 回放 主题 中 的 消息 ， 还 知道 了 如 何 使 用 rqt_bag 查 看 消息 记录 包 文 件 的 内 


容 ， 这 就 允许 你 基于 以 往 的 经 验 去 记录 数据 和 使 用 Al 算 法 或 机 器 人 算法 处 理 数 据 。 


在 接 下 来 一 草 中 ， 你 将 会 使 用 本 章 讲 过 的 这 些 工 具 可 视 化 不 同类 型 的 数据 ， 以 及 一 些 传感器 在 ROS 中 的 使 用 说 明和 它们 输出 
数据 的 可 视 化 等 。 


第 4 章 ，” 人 在 ROS 下 使 用 传感器 和 执行 器 


当 你 想到 机 器 人 ， 可 能 诈 移 是 人 形 机 器 人 ， 有 手臂 、 大 量 传感器 和 极为 复杂 的 运动 系统 。 


现在 我 们 已 经 学 习 了 如 何在 ROS 里 编写 小 程序 并 管理 它们 。 接 下 来 将 会 学 习 如 何 使 用 传感器 和 执行 器 ， 这 样 束 能 够 与 现实 世 
ARES. 


在 http://www.ros.org/wiki/Sensors 有 一 个 很 长 的 列表 清单 ， 上 面 都 是 ROS 支 持 的 设备 。 
在 本 章 中 ， 我 们 将 会 学 到 : 

` 项 目 中 可 能 会 使 用 的 廉价 且 通 用 的 传感器 

-如 Kinect 之 类 的 3D 传 感 器 和 激光 雷达 

“ 使 用 Arduino 连 接 更 多 的 传感器 和 执行 器 


在 本 章 中 将 所 有 类 型 的 传感器 都 介绍 一 思 是 不 太 现实 的 。 基 于 这 个 原因 ， 我 们 会 选择 那些 最 通用 的 和 大 多 数 使 用 者 能 够 买 得 
起 的 传感器 。 


传感器 和 执行 器 分 成 不 同 的 类 别 : 激光 雷达 、 摄 像 头 、 位 姿 估 计 设 备 等 。 这 样 会 帮助 你 更 加 快捷 地 找到 这 些 传 感 器 和 执行 
器 。 


4.1 使 用 闻 戏 杆 或 游戏 手柄 


我 敢 肯定 你 曾经 或 多 或 少 使 用 过 某 个 视频 终 冰 的 游戏 杆 或 游戏 手柄 (joystick/gamepad) 。 
游戏 杆 只 不 过 是 一 系列 的 按钮 和 电位 器 。 通 过 这 些 装置 ， 你 能 够 实现 或 控制 很 多 种 运动 模式 。 


在 ROS 系 统 中 ， 游 戏 杆 能 够 通过 改变 速度 和 方向 来 远程 控制 机 器 人 。 


在 开始 之 前 ,我 们 将 会 安 六 一 些 功 能 包 。 要 安 六 这 些 功能 包 在 Ubuntu 系 统 中 ， 首 先 执行 下 面 的 命令 : 


$ sudo apt-get install ros-hydro-joystick-drivers 


$ rosstack profile & rospack profile 


在 这 些 功 能 包 中 ， 你 能 找到 学 习 如 何 使 用 游戏 杆 的 代码 和 建立 我 们 自己 的 功能 包 的 指导 手册 。 


站 先 ， 将 游戏 手柄 连接 到 你 的 电脑 ， 使 用 如 下 代码 ， 检 查 你 的 游戏 手柄 是 否 能 够 被 识别 |: 


$ ls /dev/input/ 


我 们 会 看 到 下 面 输出 : 
by-id event0 event2 event4 event6 event8 js0 mouse0 


by-path eventl event3 event5 event7 event9 mice 
所 创建 的 端口 是 js0。 我 们 能 使 用 stest 命 令 检查 它 是 否 工作 : 


$ sudo jstest /dev/input/js0 


Axes: 0: 0 tl: 0 2: 0 Buttons: O:0f£ 1l:off 2:0ff 3:off 4:off 
5:0f£ 6:off£ 7:of£ 8:o0£f£ 9:o0f£ 10:o0f£ 


我 们 的 游戏 杆 是 罗技 (Logitech) Attack3， 有 3 个 轴 辐 输入 和 11 个 按钮 。 如 果 移 动 游戏 杆 ， 融 会 产生 数 人 变化 。 


如 果 你 能 确定 游戏 杆 的 功能 正 钊 ， 那 么 我 们 融和 直接 在 ROSS 系统 中 测试 它 的 功能 。 为 了 达到 测试 的 目的 ， 我 们 需要 使 用 oy 和 
joy node 功能 


$ rosrun joy joy node 
如 果 所 有 配置 都 正确 ， 你 会 看 到 : 


[ INFO] [1357571588.441808789]: Opened joystick: /dev/input/js0. 
deadzone : 0.050000. 


4.2 ”使 用 油光 雷达 一 一 Hokuyo URG-04Ix 


在 移动 机 器 人 中 ， 获 取 障 碍 物 的 具体 位 置 、 房 间 的 内 部 轮廓 等 信息 非常 重要 。 机 器 人 使 用 地 图 进行 导航 和 通过 未 知 的 区 域 。 
使 用 激光 雷达 (LIDAR) 正 是 为 了 实现 这 个 目的 。 这 个 传感器 专门 用 于 测量 机 器 人 和 物体 之 间 的 距离 。 


在 本 节 中 ， 你 将 会 看 到 如 何 使 用 在 机 器 人 领域 内 广泛 应 用 的 低 成 本 激光 雷达 。 本 章 的 传感器 选用 Hokuyo URG-04Ix 激 光 雷 
达 。 可 以 通过 以 下 链接 获取 更 加 详细 的 信息 : http://www.hokuyo-aut.jp/。Hokuyo 激 光 雷 达 是 能 够 实现 机 器 人 导航 和 实时 地 
图 创建 的 设备 。 


标准 的 Hokuyo URG-04Ix 是 机 器 人 领域 最 常用 的 低 成 本 激光 雷达 。 它 具有 极 佳 的 分 辩 率 而 且 非 党 容易 上 手 。 要 使 用 它 ， 首 
先 需 要 安装 激光 雷达 的 驱动 。 


$ sudo apt-get install ros-hydro-hokuyo-node 


$ rosstack profile && rospack profile 


一 旦 驱动 安 半 完毕 ， 残 应 检查 是 否 安装 正确 。 连 上 你 的 激光 雷达 然后 检查 系统 是 人 否 能 够 正确 地 检测 到 它 和 它 是 否 正确 地 配 


$ ls -l /dev/ttyACMO 

当 我 们 连 上 激光 雷达 时 ， 系 统 能 够 通过 以 下 命令 查看 它 : 

crw-rw---- 1 root dialout 166, 0 Jan 13 11:09 /dev/ttyACMO 

在 该 示例 中 ， 需 要 重新 配置 激光 雷达 以 便 ROS 能 够 访问 和 使 用 它 ， 也 束 是 说 ， 我 们 需要 获取 适当 的 访问 权限 : 
$ sudo chmod a+rw /dev/ttyACMO 

使 用 下 面 的 命令 检查 配置 是 否 成 功 : 


$ ls -1 /dev/ttyACMO 
crw-rw-rw- 1 root dialout 166, 0 Jan 13 11:09 /dev/ttyACMO 


如 果 一 切 顺利 ， 我 们 将 会 转换 到 激光 雷达 上 。 在 一 个 命令 行 窗口 中 局 动 roscore， 在 男 一 个 命令 行 窗口 中 执行 以 下 命令 : 
$ rosrun hokuyo node hokuyo node 


如 果 一 切 正常 ， 你 会 看 到 以 下 输出 : 


[ INFO] [1358076340.184643618]: Connected to device with ID: H1000484 


4.3 ”使 用 Kinect 传 感 器 查看 3D 环 境 中 的 对 象 


Kinect 传 感 器 是 一 个 局 平 的 黑 合 子 ， 其 下 方 是 一 个 可 以 活动 的 小 平台 ， 能 够 固定 在 保 子 上 或 者 你 使 用 Xbox360 的 电视 机 架子 
上 。 这 个 设备 上 面 有 三 种 传感器 能 够 帮助 我 们 完成 视 锅 和 机 器 人 任务 : 


KINECT 


for @ 


一 个 彩色 VGA 视 频 摄像 头 用 来 查看 彩色 的 世界 

. 一 个 深度 传感器 ， 一 个 红外 色 斑 投影 仪 和 一 个 单 色 CMOS 传 感 器 配合 工作 ， 获 取 物 体 的 深度 信息 并 转换 为 3D 数 据 

“ 用 于 分 离 使 用 者 的 声音 和 室内 噪声 的 多 阵列 麦克 风 
在 ROS 系 统 中 ， 我 们 将 使 用 这 两 类 传感器 : RGB 摄 像 头 和 深度 传感器 。 而 在 最 新 版 本 的 ROS 中 ， 你 甚至 能 够 用 三 种 传感器 。 
在 开始 使 用 它 之 前 ， 我 们 需要 安 浴 功 能 包 和 驱动 。 使 用 下 面 的 命令 行 来 安 痰 它们 : 


$ sudo apt-get install ros-hydro-openni-camera ros-hydro-openni-launch 


$ rosstack profile && rospack profile 


一 旦 安装 完成 ， 插 入 Kinect 传 感 器 ， 我 们 就 能 运行 节点 并 开始 使 用 它 。 在 命令 行 中 ， 启 动 roscore。 在 另外 一 个 命令 行 窗口 
中 运行 下 面 指令 : 


$ rosrun openni camera openni node 


$ roslaunch openni launch openni.launch 


如 果 一 切 正常 ， 你 不 会 看 到 任何 错误 信息 产生 。 


4.4 ”使 用 伺服 电动 机 一 一 Dynamixel 


在 移动 机 器 人 领域 里 ,伺服 电动 机 可 谓 应 用 广泛 。 此 类 执行 器 主要 用 于 移动 传感器 、 轮 子 和 机 械 辟 。 一 种 低 成 本 解决 方案 是 
使 用 RC 伺服 电动 机 (ACYL) 。 它 能 够 在 180° 的 范围 内 运动 ， 并 且 提 供 较 大 的 扭 和 矩 。 


本 书 将 会 介绍 一 种 专 为 机 器 人 设计 和 使 用 的 新 型 伺服 电动 机 ， 这 束 是 Dynamixel 伺 服 电 动机 ，。 


Dynamixel 是 由 韩国 生产 商 ROBOTIS 公 司 开 友 的 一 种 机 器 人 专用 的 线性 、 高 性 能 网 络 执行 器 。ROBOTIS 公 司 还 是 OLLO、 


Bioloid 和 和 DARwIn-OP DXL 等 机 器 人 的 设备 研发 和 供应 商 。 由 于 它们 极 佳 的 可 扩展 性 、 强 大 的 位 置 、 速 度 、 内 部 温度 、 输 入 电 
压 的 反馈 能 力 ， 还 有 简单 的 菊花 链 拓扑 简化 了 布线 连接 ， 这 些 设备 被 众多 的 企业 、 大 学 和 爱好 者 使 用 。 


在 下 面 的 图 片 中 ， 你 会 看 到 Dynamixel AX-12 和 USB 接 口 。 它 们 都 用 于 下 面 的 示例 。 


首先 ,我 们 将 会 安 闪 必需 的 功能 包 和 驱动 。 在 命令 行 窗口 输入 下 面 命 令 : 


$ sudo apt-get install ros-hydro-dynamixel-motor 


S$ rosstack profile && rospack profile 

一 旦 安 六 元 毕 ， 将 转换 器 连接 到 计算 机 并 检查 它 能 否 被 检测 到 。 通 常情 况 下 ， 它 会 在 /dev/ 文 件 夹 下 创建 一 个 以 ttyUSBX 为 
名 的 新 接口 。 如 果 你 看 到 这 个 接口 ， 那 么 一 切 融 绪 ， 丈 让 我 们 局 动 世 点 并 试 一 试 这 个 伺服 电动 机 吧 。 

在 命令 行 窗 口中 局 动 roscore， 然 后 在 另外 一 个 窗口 中 输入 下 面 命令 : 

$ roslaunch dynamixel tutorials controller manager.launch 

如 果 你 连接 了 一 个 或 多 个 电动 机 ， 那 么 你 会 看 到 电动 机 被 驱动 检测 到 。 在 该 示例 中 ， 检 测 到 了 一 个 名 为 ID6 的 电动 机 ， 并 进 
行 急 始 化 配置 。 

process [dynamixel manager-1]: started with pid [3966] 


[INFO] [WallTime: 1359377042.681841] pan tilt port: Pinging motor IDs 1 
through 25... 


[INFO] [WallTime: 1359377044.846779] pan tilt port: Found 1 motors - 1 
AX-12 [6], initialization complete. 


Dynamixel 如 何 友 送 和 接收 运动 命令 
一 旦 启动 了 controller manager.launch 文 件 ， 就 可 以 查看 主题 的 列表 。 记 住 请 使 用 下 面 命令 行 查看 主题 : 


$ rostopic list 


这 些 主 题 会 显示 配置 好 的 电动 机 状态 。 


/diagnostics 
/motor states/pan tilt port 
/rosout 


/rosout agg 


如 果 使 用 rostopic echo 命 令 查 看 /motor states/pan tilt port， 你 会 看 到 所 有 电动 机 的 状态 。 在 我 们 的 示例 中 ， 仪 仪 有 一 
个 ID6 的 电动 机 。 然 而 ， 我 们 不 能 使 用 这 些 主题 来 驱动 电动 机 ， 所 以 我 们 需要 加 载 另 一 个 launch 文 件 来 做 这 些 工 作 。 


这 个 launch 文 件 会 创建 驱动 电动 机 所 必需 的 主题 : 


$ roslaunch dynamixel tutorials controller spawner.launch 
这 时 ， 在 主题 的 清单 中 会 出 现 两 个 新 的 主题 ， 其 中 一 个 新 主题 将 会 用 来 驱动 伺服 电动 机 ， 如 下 所 示 : 


/diagnostics 

/motor states/pan tilt port 
/rosout 

/rosout agg 

/tilt controller/command 


/tilt controller/state 


我 们 使 用 /tilt controller/command 通 过 rostopic pub 命 令 友 布 主题 来 驱动 电动 机 。 首 先 查 看 主题 的 字段 和 类 型 ， 需 要 输入 


下 面 命令 行 : 


$ rostopic type /tilt controller/command 


std msgs/Float64 


正如 你 所 见 ， 这 是 一 个 Float64 类 型 的 变量 。 这 些 变 量 通过 一 个 以 弧度 表示 的 位 置 指令 来 驱动 电动 机 。 所 以 通过 如 下 指令 发 
布 主题 : 


$ rostopic pub /tilt controller/command std msgs/Float64 -- 0.5 
一 旦 命令 下 友 出 去 ， 你 会 看 到 电动 机 转动 ， 但 是 它 会 停止 在 0.5rad 或 28.6478898* 上 。 
创建 和 使 用 伺服 电动 机 示例 


现在 ,我 们 将 会 向 你 展示 如 何 使 用 节点 来 驱动 电动 机 。 在 你 的 /chapter4 _tutorials/src 文 件 夹 下 创建 一 个 亲 
c4 example4.cpp， 并 输入 下 面 代 人 码 : 


#include<ros/ros.h> 
#include<std msgs/Float64.h> 
#include<stdio.h> 


using namespace std; 


class Dynamixel { 
private: 
ros: :NodeHandle n; 
ros: #Publisner pub tii 
public: 
Dynamixel () ; 
int moveMotor (double position); 


}; 


Dynamixel: :Dynamixel () { 
pub n = n.advertise<std msgs::Float64>("/tilt controller/ 
command",1); 


| 


int Dynamixel: :moveMotor (double position) 
std_msgs::Float64 aux; 
aux.data = position; 
pub n.publish (aux) ; 
return 1; 


int main(int argc,char** argv) 
ros::init (argc, argv, "example4 move motor") ; 
Dynamixel motors; 


float counter = -180; 
ros::Rate loop rate(100) ; 
while (ros: :ok() ) 


{ 


if (counter < 180) 


{ 


motors.moveMotor (counter*3.14/180) ; 
counter++; 

Jelse{ 
counter = -180; 


} 


loop _rate.sleep() ; 


} 


这 个 节操 会 驱动 电动 机 在 -180*~180° 之 间 不 断 运 动 。 这 是 一 个 非 弟 简单 的 例子 ， 但 你 能 够 使 用 它 进行 复杂 的 运动 或 者 控制 
更 多 的 电动 机 。 我 们 假设 你 能 够 理解 这 些 代码 ， 所 以 没有 必要 进行 过 多 的 解释。 请 注意 你 是 在 同 /tilt_controller/command 主 题 
上 友 布 数据 ， 这 融 是 电动 机 的 名 称 。 


4.5 ”使 用 Arduino) 东 加 更 多 的 传感器 和 执行 器 


Arduino 是 一 种 由 灵活 、 易 于 开发 的 软 硬 件 组 成 的 开源 电子 设计 平台 。 它 用 于 艺术 家 、 设 计 人 员 、 爱 好 者 和 所 有 喜爱 创造 交 
互 式 物体 和 环境 的 人 。 右 图 是 一 个 Arduino 开 上 友 板 : 


ROS 能 通过 rosserial 功 能 包 使 用 此 类 设备 。 通 单 Arduino 使 用 串口 和 电脑 连接 ， 数 据 传输 也 是 通过 串口 。 你 可 以 通过 
rosserial 使 用 大 量 支 持 串 口 连接 的 设备 ， 例 如 GPS、 何 服 控制 器 等 。 


首先 输入 下 面 命令 ， 安 委 功 能 


$ sudo apt-get install ros-hydro-rosserial-arduino 


$ sudo apt-get install ros-hydro-rosserial 


然后 到 catkin 工 作 空间 ， 先 复制 rosserial 库 。 用 下 面 代码 创建 rosserial 消 息 并 编译 ros lib: 


$ cd ~/dev/catkin ws/src/ 
$ git clone https://github.com/ros-drivers/rosserial.git 


$ cd ~/dev/catkin ws/ 


$ catkin make 
$ catkin make install 


$ source install/setup.bash 


好 的 ， 我 们 假设 你 已 经 安装 了 Arduino IDE。 如 果 还 没有 ， 请 根据 http://arduino.cc/en/Main/Software 描 述 的 步骤 进行 安 
对 于 Ubuntu12.04， 可 以 通过 下 面 命令 进行 安装 : 


it 


$ sudo apt-get update && sudo apt-get install arduino arduino-core 


当 你 将 所 有 功能 包 和 1DE 都 安 半 好 ， 残 能 够 从 rosserial 功 能 包 复 制 ros lib 到 sketchbook 库 文件 夹 下 。 这 个 库 的 文件 夹 会 在 你 
的 计算 机 运行 Arduino 1DE 之 后 创建 。 然 后 ， 需 要 运行 make libraries.py: 


$ cd ~/sketchbook/libraries 


$ rosrun rosserial arduino make libraries.py . 


创建 使 用 Arduino 的 示例 


现在 我 们 将 从 IDE 上 传 示例 代码 到 Arduino。 和 选择 Hello World 示 例 并 上 传 Sketch。 


Pe Edit Sketch Tools Help 


New Ctrl 十 


Open... Ctrl 二 


Sketchbook p 


Fl 


Close 


Save Ctri+S 


Save As... Ctri+Shit+SsS 


Upload to I/O Board Ctrl+U 


Page Setup Ctril+Shift+P 


Print Ctri+P 


Preferences Ctri+Comma 


Quit Ctri+Q 


nh. advertise (chatter): 


bh 


vold Loop () 

1 
str_msg. data = hello; 
chatter.publish( &str_msq }; 
nh.spinOnce(); 
delay (1000); 


Ctri+Ww 


2 .Digital 

3.Analog 
4,.Communication 
5.Control 
6.Sensors 

7 Display 
8.Strings 


ArduinolSP 


Aei pec array test 


ArduinoTestSuite > button exampl 


EEPROM 


float64 test 


Ethernet 


Firmata time test 


LiquidCrystal 


fF Matrix 


这 些 代码 和 下 面 的 代码 都 很 类 似 。 其 中 ， 在 include 行 中 包含 了 ros.h 头 文件 。 它 就 是 之 前 安装 的 rosserial 库 。 还 能 够 通过 主 
题 查 看 一 个 库 发 送 的 消息 ， 在 本 例 中 是 std_msgsString 类 型 。 


下 面 代码 都 保存 在 c4_example5_1.ino 文 件 中 : 


SD 
Servo 
SP| 


stepper 


#include <ros.h> 
#include <std msgs/String.h> 


ros::NodeHandle nh; 


std_msgs::String str_msg; 
ros::Publisher chatter("chatter", &str msg) ; 


char hello[19] = "chapter4 tutorials"; 
void setup () 


nh.initNode () ; 
nh.advertise (chatter) ; 


void loop () 


str msg.data = hello; 
chatter.publish( &str msg ); 
nh.spinOnce () ; 

delay (1000) ; 


| 


Arduino 代 码 分 为 两 个 国 数 : setup () 和 loop () 。setup () 函数 执行 一 次 ， 通 常用 于 开发 板 配 置 初始 化 ， 之 后 
loop () 函数 连续 运行 。 在 setup () 水 数 中 ， 设 定 了 主题 的 名 称 。 本 示例 中 名 称 是 chatter。 现 在 我 们 需要 局 动 一 个 节点 监听 
疹 口 并 通过 Arduino 向 ROS 了 网 络 中 友 布 主题 。 在 命令 行 窗 口中 输入 下 面 的 命令 ， 记 得 运行 roscore。 


$ rosrun rosserial python serial node.py /dev/ttyACMO 
现在 可 以 通过 rostopic echo 命 令 看 到 Arduino 发 出 的 消息 : 

$ rostopic echo chatter 

在 命令 行 窗 口中 看 到 下 面 数据 : 


data: chapter4 tutorials 


最 后 的 示例 是 从 Arduino 同 计算 机 友 送 数据 。 现 在 我 们 将 会 使 用 一 个 示例 ， 其 中 Arduino 会 订阅 一 个 主题 并 改变 连接 到 13 号 
引 脚 上 的 LED 灯 状态 。 示 例 的 名 称 是 blink， 你 可 以 从 Arduino 1DE 的 标题 栏 上 直接 查找 File|Examples|ros lib|Blink。 


下 面 的 代码 片段 已 经 保存 在 c4_example5_2.ino 文 件 中 : 


#include <ros.h> 
#include <std msgs/Empty.h> 


ros::NodeHandle nh; 
void messageCb( const std msgs::Empty& toggle msg) { 


digitalWrite(13, HIGH-digitalRead(13)); // blink the led 


| 


ros: :Subscriber<std msgs::Empty> sub ("toggle led", &messageCb ) ; 
void setup () 
{ 

pinMode (13, OUTPUT) ; 


nh.initNode() ; 
nh.subscribe (sub) ; 


void loop () 


{ 


nh.spinOnce() ; 
delay (1); 


注意 启动 节点 与 Arduino 板 进行 通信 : 
$ rosrun rosserial python serial node.py /dev/ttyACMO 


现在 ， 如 果 你 想 改 变 LED 灯 的 状态 ， 可 以 使 用 rostopic pub 命 令 友 布 一 个 新 的 状态 : 


$ rostopic pub /toggle led std msgs/Empty "{}" -once 
publishing and latching message for 3.0 seconds 


(RERINLEDNSE TAS. MRLE RA, PACE. AS HKNERAS, (RBA KRHEA: 


$ rostopic pub /toggle led std msgs/Empty "{}" -once 


publishing and latching message for 3.0 seconds 


现在 你 能 够 在 ROs 中 使 用 所 有 Arduino 的 设备 了 。 这 非 昔 有 用 ， 因 为 你 可 以 使 用 各 种 廉价 的 传感器 和 执行 器 委 备 你 的 机 器 人 
Js 


当 我 们 开始 编写 本 章 的 时 候 ， 我 们 发 现 有 些 Arduino 设 备 不 支持 tossetial ， 例 如 Atduino Leonardo。 因 此 在 使 用 本 功能 包 之 
前 ， 最 好 小 心 你 所 选 的 设备 。 


我 们 在 使 用 Arduino UNO R3、Arduino Mega, Arduino Duemilanove 或 Atduino Nano 的 时 候 ， 没 有 出 过 任何 问题 。 


46 在 Arduino 上 使 用 超声 波 传感器 


使 用 一 个 MO 开 上 友 板 ， 如 Arduino， 可 以 连接 多 个 传感器 和 执行 器 。 在 本 节 中 ， 我 们 将 对 Arduino 编 程 来 控制 HC-SR04， 将 
超声 波 距 离 传 感 器 用 于 ROS。HC-SR04 是 一 蒜 低 成 本 传感器 ， 通 剃 用 于 测量 机 器 人 和 障碍 物 之 间距 离 。 


这 个 设备 包括 一 个 印刷 电路 板 (PCB) 与 两 个 类 似 麦 克 风 和 压 电 耳机 的 压 电 转换 器 ， 但 工作 在 超声 波 范 围 。 其 中 一 个 传感器 
是 友 射 器 ， 另 一 个 是 接收 器 。 这 种 传感器 测量 距离 的 原理 基于 动物 的 回声 定位 系统 ， 如 蝙蝠 。 传 感 器 会 友 出 超声 波 ， 如 果 直 到 障 
碍 ， 超 声波 会 反射 ， 它 将 返回 传感器 。 根 据 空气 中 的 声速 ， 我 们 将 测量 声音 友 射 和 接收 之 间 的 时 间 郑 来 计算 障碍 物 的 距离 。 


HC-SR04 传 感 器 有 四 个 引 脚 ，VCC 将 连接 到 ?V， 将 GND 连 接 到 GND， 触 友 器 (TRIG) 将 连接 到 7 号 引 脚 ，ECHO 连 接 到 3 
号 引 脚 。 触 发 器 用 来 激活 发 射 超声 波 。 当 我 们 在 7 号 引 脚 中 输出 逻辑 高 电 平 ， 传 感 器 会 友 出 超声 波 。 如 果 传 感 器 接收 超声 波 ， 回 
声 引 脚 电压 将 在 一 个 较 高 的 水 平 ， 如 下 图 所 示 : 
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使 用 Arduino IDE 打 开 c4 examples5 2.ino 并 上 传 到 开 友 板 。 代 人 码 如 下 : 


#include <ros.h> 
#include <std msgs/Int16.h> 


ros::NodeHandle nh; 

std _ msgs::Intl6 range; 

ros::Publisher range pub("range", &range) ; 
const int trigpin = 7; 

const int echopin = 3; 

long duration = 0; 


void setup () 


nh.initNode () ; 


nh.advertise (range pub}; 


| 


void loop() 


{ 
range.data = ping(); 
range pub.publish(&range) ; 
nh.spinOnce() ; 
delay(100) ; 


long ping() 


{ 


// Send out PING))) signal pulse 
pinMode(trigpin, OUTPUT) ; 
pinMode (echopin, INPUT) ; 
digitalWrite(trigpin, LOW); 
delayMicroseconds (2) ; 
digitalWrite(trigpin, HIGH); 
delayMicroseconds (10) ; 
digitalWrite(trigpin, LOW); 
//Get duration it takes to receive echo 
duration = pulseIn(echopin, HIGH); 
//Convert duration into distance 
return duration /29/2; 


这 里 包括 ros lib 和 std msgslnt16 库 ，ros 句 柄 声明 和 std msgs::Int16 命 名 为 "range"。 在 这 个 主题 中 ，Arduino 将 发 送 最 
近 障 碍 的 距离 信息 : 


std msgs: :Int16 range; 
ros::Publisher range pub("range", &range) ; 


在 setup () 函数 中 ， 初 始 化 ROS 节 点 并 发 布 消息 ， 如 下 所 示 : 


nh.initNode(); 
nh.advertise (range pub); 


在 Arduino 的 loop () 函数 ，ping () 函数 被 调用 来 管理 超声 波 传 感 器 、 距 离 计 算 ， 然 后 以 cm 为 单位 友 布 距离 信息 。100 
室 秒 的 延迟 周期 ， 即 以 接近 10Hz 人 速度 生成 数据 : 


range.data = ping(); 
range pub.publish(&range) ; 
nh.spinOnce () ; 


delay (100); 


ping () 函数 声明 引 脚 7 作为 数字 输出 以 及 引 脚 3 为 数字 输入 。 我 们 在 引 脚 7 输出 低 电 平 以 确保 没有 声 瘟 友 出 并 保持 通道 的 清 
洁 。 然 后 ， 写 一 个 数字 高 电 平 持续 10 微 秒 。 


之 后 ，Arduino 的 pulseln() 遂 数 测量 时 间 ， 直 到 引 脚 处 于 高 电 平 ， 表 明 接 收 到 超声 波 。 这 个 遂 数 返回 以 ms 为 单位 的 时 
间 。 脉 冲 的 持续 时 间 乘 以 声音 的 速度 来 计算 距离 。 需 要 注意 的 是 距离 除 以 2， 因 为 声音 脉冲 经 过 的 路 径 是 距离 的 2 做 。 首 先 ， 饭 
遇 到 障碍 ， 然 后 回 到 传感器 。 


局 动 这 个 节操 ， 我 们 需要 在 终 吴 运行 roscore。 在 一 个 新 的 终端 ， 我 们 将 运行 rosserial 节 点 与 Arduino 通 信 : 
$ rosrun rosserial python serial node.py /dev/ttyACM™MO 
现在 ,我 们 为 在 ROS 中 使 用 超声 小传 感 器 做 好 了 准备 。 


有 趣 的 是 在 水 下 机 器 人 领域 ， 声 呐 是 非常 基础 的 找 术 之 一 ， 它 扮演 的 角色 与 地 面 机 器 人 中 的 激光 测 距 传感器 相同 。 由 于 声音 
在 水 中 传播 特性 比 光 更 好 ， 因 此 在 水 下 往往 采用 声响 传感器 来 检测 障碍 、 构 建 地 图 以 及 确定 位 置 。 
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如 果 你 上 传 的 Arduino 代 码 正确 ， 并 且 正 常 启 动 serial_node.py， 我 们 可 以 在 主题 列表 中 看 到 主题 名 /range: 
$ rostopic list 


可 以 像 往 党 一 样 使 用 rostopic 合 看 主题 的 类 型 : 


$ rostopic type /range 
std msgs/Int16 


使 用 下 面 命 令 查 看 消息 中 包含 的 数据 : 
$ rostopic echo /range 


你 可 以 使 用 这 个 传感器 ， 将 它 指向 不 同 距离 的 对 象 ， 查 看 距离 信息 。 


48 ”使 用 10 目 由 度 低 成 本 惯性 测量 模 组 |MU 


在 本 节 中 ， 我 们 将 会 学 习 使 用 一 种 具有 10 自 由 度 (DoF) 的 低 成 本 IMU 传 感 器 。 这 个 传感器 和 Xsens MTi 非 党 相似 ， 都 有 加 
速度 计 (x3) 、 磁 场 强度 计 (x3) 、 和 气压 计 (x1) 和 陀螺 仪 (x3) 。 通 过 12C 接 口 能 够 轻松 地 控制 它 ， 并 且 在 本 示例 中 ， 它 会 
与 Arduino Nano 进 行 连 接 (http://arduino.cc/en/Main/ArduinoBoardNano) 。 


这 个 传感器 的 用 途 也 非常 相似 。 购 买 Xsens 大 概要 花费 $2000， 对 于 一 般 的 用 户 来 说 是 比较 昂贵 的 ， 而 我 们 本 节 中 要 介绍 的 
传感器 只 需要 大 约 $20， 这 个 传感器 杀 民 的 价格 让 它 能 够 用 在 很 多 项 目 上 。 通 过 下 面 的 图 片 你 能 看 到 这 个 传感器 。 它 很 注 并 且 只 


有 十 几 个 元 件 。 


Oo = aa 


sar 
Sf 
25 n 
g 
nL 
~ 
G? 
287. 
a 
~ 


这 块 板子 具有 以 下 传感器 : 


- ADXL345: 这 是 一 个 高 精度 (13-bit) 三 轴 加 速度 计 ， 最 高 测量 范围 为 十 16g。 这 个 传感器 广泛 地 用 于 移动 设备 终 


Sw Lb 
So CHE 


够 测量 静态 的 重力 加 速度 用 于 倾角 测量 用 途 ， 同 时 也 能 够 测量 由 于 运动 和 冲击 造成 的 动态 加 速度 。 
- HMC5883L: 这 个 传感器 用 来 测量 低 磁 场 强度 ， 并 配 有 数字 接口 ， 如 低 成 本 罗盘 和 磁 强 计 等 。 


BMP085: 这 是 用 于 移动 应 用 的 先进 高 精度 大 气压 力 传 感 器 。 它 性 能 强大 ， 有 具有 最 高 0.03hPa 的 绝对 精度 和 3nA 的 超 低 功 


-13G4200D: 这 是 高 精度 (16-bit) 三 轴 陀 螺 仪 ， 测 量 范 围 高 达 2000 度 每 秒 (dps) 。 这 个 陀螺 仪 能 够 同时 对 三 个 轴 的 转动 


量 进行 测量 。 


正如 前 面 所 说 ， 这 块 板 子 通过 12C 协 议 进 行 控制 。 我 们 能 够 使 用 Arduino 来 控制 它 。 在 下 图 中 ， 你 能 看 到 如 何 连接 Arduino 
和 |MU 两 块 板子 : 


A4<-->SDA 
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唯一 需要 做 的 事情 是 连接 四 根 线 让 板子 工作 。 从 Arduino 的 GND 和 5V 分 别 连 线 到 IMU 的 GND 和 VCC。 


串口 数据 链 路 (SDL) 需要 连接 到 模拟 量 4 号 引 脚 ， 串 口 时 钟 (SCK) 需要 连接 到 模拟 量 5 号 引 脚 。 如 果 这 些 引 脚 连 接 错 
误 ，Arduino 残 无 法 和 传感器 通信 。 


4.9 ”GPS 的 使 用 


全 球 定 位 系统 (GPS) 是 一 种 基于 空间 的 系统 ， 它 能 在 任何 天 气 、 地 球 表面 及 其 附近 的 任何 地 区 提供 关于 位 置 和 时 间 的 信 
息 。 你 必须 与 四 个 全 球 定位 系统 卫星 之 间 有 畅通 的 通道 才能 获得 有 效 数据 。 


通过 GPS 获 得 的 数据 符合 由 NMEA 建 立 的 通信 标准 和 不 同类 型 的 协议 。 在 这 里 ， 我 们 可 以 得 到 接收 器 的 位 置信 息 。 查 阅 
NMEA 所 有 信息 的 类 型 你 可 以 访问 http://www.gpsinformation.org/dale/nmea.htm。 


GPS 中 最 有 趣 的 信息 是 在 GGA 中 ， 它 们 提供 通用 的 Fix 数 据 以 及 GPS 的 3D 位 置 。 下 面 是 这 个 sentence 和 每 一 块 的 解释 : 


SGPGGA, 123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47 
Where: (解释 ) 
GGA Global Positioning System Fix Data (全 球 定 位 修正 数据 ) 
123519 Fix taken at 12:35:19 UTC (修正 于 ) 
4807.038,N Latitude 48 deg 07.038' N (纬度 ) 
01131.000,E Longitude 11 deg 31.000' E (2È) 
1 Fix quality: 0 = invalid (修正 质量 ) 


1 = GPS fix (SPS) (修正 ) 

2 = DGPS fix (修正 ) 

3 = PPS fix (修正 ) 

4 = Real Time Kinematic (实时 运动 学 ) 

5 = Float RTK 

6 = estimated (dead reckoning) (2.3 

feature) (估计 航路 推算 ) 

7 = Manual input mode (手动 输入 模式 ) 

8 = Simulation mode (仿真 模式 ) 
08 Number of satellites being tracked (已 跟 足 卫星 数 ) 
0 .9 Horizontal dilution of position (水 平 位 置 精度 ) 
545.4,M Altitude, Meters, above mean sea level (BE, KX, 平均 海平 面 ) 
46.9,M Height of geoid (mean sea level) above WGS84 


ellipsoid (大 地 水 准 面 高 (平均 海平 面 ) 高 于 WGS84 HEIR) 
(empty field) time in seconds since last DGPS update 
( 自 上 一 次 DGPS 更 新 时 间 间 阳 , 以 秒 为 单位 ) 
(empty field) DGPS station ID number (DGPS 基站 ID 号 ) 
*47 the checksum data, always begins with * ( 校 验 和 数据 ， 总 是 始 于 ) 


GPS 接 收 器 不 同 ， 它 的 性 能 和 精度 也 不 同 。 通 用 廉价 的 GPS 可 以 应 用 在 不 同 的 地 方 ， 比 如 说 无 人 飞行 器 (UAV) 。 它 们 的 误 
孝 可 能 在 几米 。 也 可 以 用 比较 昂贵 的 GPs 设 备 把 它 配制 成 差分 GPS3， 或 者 工作 在 实时 动态 (RTK) 模式 ， 第 二 个 GPS 在 一 个 可 知 
的 位 置 给 第 一 台 GPs 发 送 校正 数据 。 这 种 GPS 可 以 得 到 位 置 误差 在 10cm 以 下 的 信息 。 


GPS 通 过 串口 协议 向 计算 机 或 者 微型 控制 器 (Arduino) 友 送 接收 的 数据 。 我 们 可 以 用 TTL、RS232 通 过 USB 适 配器 很 容 
易 地 与 计算 机 相连 。 这 一 节 ， 我 们 将 用 到 廉价 的 GPS (EM-406a) 和 一 个 精确 的 系统 ， 如 在 实时 动态 模式 下 的 GR-3Topcon。 
将 会 看 到 在 相同 的 驱动 下 ， 我 们 可 以 从 这 两 个 设备 中 得 到 经 度 、 纬 度 和 高 度 。 


为 了 用 ROS 控 制 GPS 传 感 器 ， 我 们 将 用 下 面 的 命令 安装 NMEA GPS 的 驱动 包 (不 要 忘 了 在 那 之 后 运行 rosstack 和 rospack 配 
置 文件 ) : 


$ sudo apt-get install ros-hydro-nmea-gps-driver 


$ rosstack profile & rospack profile 


运行 nmea gpst drivers.py 文 件 ， 局 动 GPS 驱 动 。 同 时 我 们 要 声明 两 个 参数 : 与 GPS 相 接 的 端口 以 及 波 特 率 。 


$ rosrun nmea gps driver nmea gps driver.py port:=/dev/ttyUSBO _ 
baud: =4800 


EM-406a GPS 和 Topcon GR-3 的 波 特 率 分 别 是 4800Hz 和 115200Hz， 如 果 我 们 想 在 ROS 下 用 它们 ， 必 须 像 下 面 命 令 行 中 那 
样 声明 _baud 人 参数 。 


$ rosrun nmea gps driver nmea gps driver.py port:=/dev/ttyUSBO | 
baud: =115200 
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在 机 器 人 中 使 用 传感器 和 执行 器 是 非常 重要 的 ， 因 为 这 是 和 现实 世界 进行 互动 的 唯一 办 法 。 在 本 章 中 ， 我 们 进一步 学 习 了 如 
何 使 用 、 配 置 和 检查 一 些 通用 的 传感器 和 执行 器 ， 这 些 设备 在 全 世界 的 机 器 人 领域 里 广泛 使 用 。 可 以 确定 ， 如 果 想 使 用 其 他 类 型 
的 传感器 ， 你 都 能 够 从 网 络 和 和 ROS 文档 中 找到 相关 信息 并 轻松 地 使 用 。 


在 我 看 来 ，Arduino 是 非常 有 趣 的 设备 。 因 为 有 了 它 ， 你 能 够 向 计算 机 添加 更 多 的 设备 和 各 种 廉价 的 传感器 ， 能 够 很 侧 单 轻 
松 地 在 ROS 框 架 下 使 用 它们 。Arduino 还 有 非常 大 的 社区 ， 这 样 你 能 够 找到 各 种 传感器 的 信息 ， 绝 对 能 够 覆盖 你 所 能 想象 到 的 各 
种 应 用 。 


最 后 需要 说 明 的 是 ， 激 光 雷 达 在 后 面 的 章节 中 是 最 有 用 的 传感器 。 因 为 这 是 应 用 导航 功能 包 集 必须 使 用 的 设备 。 导 航 功 能 
集 依赖 于 激光 雷达 提供 的 高 精确 度 和 高 频率 的 数据 。 在 下 一 章 中 ， 你 会 看 到 如 何 进 行 机 器 人 建 模 ， 在 rviz 中 对 机 器 人 进行 可 视 
化 ， 并 在 ROS 已 经 集成 的 Gazebo 仿 真 环境 中 加 载 和 使 用 它 。 


第 5 草 TAN 


ROS 对 计算 机 视 党 提供 了 基本 的 支持 。 首 先 ，ROS 在 处 理 视 党 任务 时 提供 给 我 们 包含 了 用 于 各 类 摄像 头 和 协议 的 驱动 程序 ， 
尤其 是 FireWire (IEEE1394a 或 IEEE1394b) 接口 的 摄像 头 。 图 像 管 道 提 供 了 摄像 头 标定 过 程 、 扭 曲 矫 正 、 颜 色 解 码 和 其 他 底层 
操作 。 对 于 更 为 复杂 的 任务 ， 你 可 以 使 用 OpenCV、cv_bridge 和 image transport 库 与 其 连接 ， 订阅 和 发 布 图像 主 题 来 进行 处 
理 。 最 后 ， 还 有 一 些 能 实现 如 物体 识别 、 增 强 现实 、 视 觉 里 程 计 等 算法 的 功能 包 。 


尽管 ROS 的 功能 包 集成 了 FireWire 摄 像 头 的 驱动 ， 而 对 于 USB 和 高 速 网 络 摄像 头 ， 支 持 这 些 协 议 也 不 困难 。UsB 摄 像 头 不 但 
价格 低廉 ， 而 且 也 很 容易 买 到 ， 在 本 草 中 ， 我们 将 讨论 这 其 中 的 一 些 ， 此 外 我 们 还 提供 了 一 个 使 用 了 OpenCV 视 频 捕 捉 API 的 驱 
动 ， 访 驱动 能 够 无 颖 集成 于 图 像 管道 中 。 


我 们 会 详细 解释 摄像 头 标定 以 及 包含 在 图 像 管道 中 标定 的 结果 。ROs 能 够 提供 GUI 来 帮助 我 们 使 用 标定 模板 进行 摄像 头 标 
定 。 然 后 ， 我 们 会 介绍 双 目 摄像 头 ， 以 及 如 何 管理 那些 比 双 目 摄像 头 复杂 得 多 的 市 有 两 个 甚 全 更 多 摄像 头 的 委 置 。 我 们 将 会 进 一 
步 介绍 这 一 切 是 如 何 集成 在 系统 中 的 。 例 如 ， 双 目 视 完 将 会 让 你 从 某 种 程度 上 或 某 个 学 围 内 获取 环境 的 深度 信息 。 因 此 ,我们 将 
会 看 到 如 何以 点 云 的 形式 来 查看 这 些 信息 ， 如 何 让 摄像 头 能 够 获得 最 佳 的 效果 和 质量 。 


ROS 带 有 的 图 像 管道 能 简化 从 摄像 头 获取 的 源 图 像 (RAW image) 到 单 色 (WE) 图 像 和 彩色 图 像 的 转换 过 程 。 这 在 某 些 
时 候 意 味 着 如 果 图 像 被 编码 为 拜耳 模式 ， 那 么 束 需 要 对 源 图 像 进行 去 拜耳 化 (debayer) 。 这 对 于 高 质量 的 FireWire 摄 像 头 来 说 
非常 弟 见 。 如 果 摄 像 头 已 经 被 标定 过 ， 那 么 这 些 标定 信息 会 补 用 于 修正 图 像 。 修 正 图 像 措 的 是 使 用 在 标定 过 程 中 计算 出 的 失真 系 
效 对 图 像 扭曲 进行 更 正 。 最 后 ， 对 于 双 目 图 像 ， 因 为 我 们 有 左右 摄像 头 乙 间 的 基线 ， 所 以 能 够 计算 出 允许 我 们 获取 深度 信息 的 视 
帮 图 像 ， 并 在 调整 之 后 获得 3D 点 云图 像 。 这 里 我 们 将 会 给 出 一 些 调 整 图 像 的 建议 ， 当 然 这 对 于 低 精 度 摄 像 头 来 说 可 能 比较 困 


难 ， 而 且 有 时 候 要 以 获得 较 高 的 标定 结果 为 前 提 。 


最 后 ， 通 过 ROS 中 的 OpenCV (只 有 2.x 版 本 ，3.x 版 本 还 不 支持 ) ， 我 们 能 够 应 用 大 量 的 计算 机 视觉 和 机 器 学 习 算 法 ， 或 者 
运行 一 些 已 经 在 OpenCV 库 中 包含 的 算法 和 示例 。 在 这 里 ， 我 们 将 不 会 深入 探讨 OpenCV AP1， 因 为 这 超出 了 本 书 的 范围 。 但 
是 ， 我 们 会 建议 读者 阅读 一 下 在 线 文档 (http://docs.opencv.org) 或 者 一 些 关于 计算 机 视觉 和 OpenCV 的 书籍 。 这 里 ， 我 们 将 
通过 一 个 示例 ， 向 你 简单 介绍 如 何在 节点 中 使 用 OpenCV， 具 体 是 用 特征 提取 和 匹配 方法 计算 两 幅 图 像 之 间 的 单 应 性 。 此 外 ， 本 
章 将 通过 完成 一 个 教程 在 ROS 中 设置 并 运行 一 个 视觉 里 程 计 。viso2 ros 是 libviso2 视 觉 里 程 库 的 封装 ， 其 使 用 的 双 目 视觉 通过 在 
一 个 支撑 杆 上 固定 两 个 便宜 的 网 络 摄像 头 实现 。 其 他 视觉 里 程 计 库 也 会 讲 到 ， 例 如 fovis。 包 括 一 些 使 用 它们 的 建议 以 及 如 何 提 
高 RGBD 传 感 器 的 效果 ， 如 Kinect。 甚 至 还 有 传感器 融合 或 在 单 目 视觉 方面 的 一 些 信息 . 


5.1 ”连接 和 运行 摄像 头 


我 们 需要 按 一 定 的 步 又 进行 操作 ， 第 一 步 是 将 摄像 头 与 计算 机 连接 ， 运 行 驱动 并 检查 是 否 能 在 ROS 中 获取 图 像 。 其 实在 使 用 
ROS 之 前 ,最 好 使 用 其 他 工具 来 检查 摄像 头 是 人 否 被 操作 系统 正确 识别 ， 在 这 里 可 以 使 用 Ubuntu 友 行 版 。 我 们 将 从 FireWire 摄 像 
头 讲 起 ， 因 为 ROS 系 统 对 它 的 支持 更 好 ， 然 后 册 学 习 USB 摄 像 头 。 


5.2 ”使 用 OpenCV 制 作 USB 摄 像 头 驱 动 程序 


里 然 我 们 之 前 已 经 提 到 了 两 种 方 去 ， 但 本 书 还 是 会 提供 我 们 目 己 的 USB 报 像 头 驱动 。 驱 动 是 通过 调用 OpenCV 的 
cv:VideoCapture 类 来 实现 的 。 它 能 够 驱动 摄像 头 并 且 一 旦 它们 受 摄 像 头 固件 文 持 ， 人 允许 我 们 去 修改 它 的 一 些 参数 。 事 实 上 ， 
使 用 usb_cam 不 可 能 实现 这 些 功能 ， 因 为 Cameralnfo 消 息 是 不 可 用 的 。 而 在 gscam 中 ， 我 们 可 以 进行 更 多 的 控制 。 我 们 可 以 在 
ROS 中 改变 摄像 头 配 置 ， 并 碍 看 如 何 上 友 布 摄像 头 图 像 和 信息 。 根 据 摄像 头 图 像 的 读 取 方 式 ， 我 们 有 两 种 使 用 DpenCV 实 现 摄 像 头 
驱动 的 方法 。 第 一 个 是 根据 每 秒 钟 给 定 的 图 像 帧 (Frames Per Second, FPS) 进行 轮 询 ; 第 二 个 是 我 们 能 够 根据 像 FPS 的 周期 
设 定 一 个 计时 器 ， 并 在 计时 器 的 回调 函数 中 完成 实际 的 读 取 工 作 。 由 于 我 们 的 方法 依赖 于 FPS9， 因 此 在 CPU 的 消耗 上 要 比 其 他 方 
法 好 。 无 论 如何 ， 当 OpenCV 读 取 功 能 被 阻 断 时 ， 是 无 法 激活 轮 询 的 ， 而 且 在 图 像 准备 好 之 前 ， 其 他 进程 会 占据 CPU。 通 常情 
况 下 ， 在 FPS 较 大 时 ， 最 好 使 用 轮 询 的 方法 ， 这 样 我 们 无 需 承 担 使 用 计时 器 和 回调 消 数 所 造成 的 时 间 损 失 。 在 FPS 较 低 时 ， 计 时 
器 方法 和 轮 询 方法 就 差不多 了 ， 而 前 者 的 代码 更 清晰 。 我 们 建议 读者 能 够 比较 这 两 种 实现 方式 ， 它 们 分 别 存 储 在 
src/camera_polling.cpp 和 src/camera_timer.cpp 文 件 中 。 为 了 节省 大 家 的 时 间 ， 我 们 在 这 里 介绍 基于 计时 器 的 实现 方法 。 在 
src/camera.cpp 文 件 中 最 终 实现 的 驱动 也 使 用 了 计时 器 方法 。 请 注意 在 最 终 的 驱动 程序 中 还 包括 了 我 们 将 在 后 面 章节 中 介绍 的 
摄像 头 信息 管理 。 

在 功能 包 中 ， 我 们 必须 设置 OpenCV、ROS Image 消 息 库 和 相关 功能 包 的 依赖 项 。 如 下 所 示 : 

<depend package="sensor msgs"/> 
<depend package="opencv2"/> 


<depend package="cv_bridge"/> 
<depend package="image transport"/> 


因此 ， 在 srccamera timer.cpp 文 件 中 我 们 要 包含 以 下 头 文件 : 


#include <ros/ros.h> 

#include <image transport/image transport .h> 
#include <cv_ bridge/cv_ bridge.h> 

#include <sensor msgs/image encodings.h> 
#include <opencv2/highgui/highgui.hpp> 


image_transport API 多 许 我 们 使 用 多 种 传输 格式 友 布 图 像 ， 其 中 包括 各 种 压缩 图 像 格式 和 在 ROSS 系统 中 以 插件 形式 安 净 的 
各 种 编 解 码 器 ， 例 如 compressed 和 theora。 上 面 的 cv_bridge 字 段 用 于 从 OpenCV 图 像 到 ROSS Image 消息 的 转换 ， 其 中 有 灰 
度 / 斋 色 转换 时 还 会 用 到 sensor_ msgs 进 行 图 像 编 码 。 最 后 为 了 使 用 cv::VideoCapture 还 需要 OpenCV (opencv2) 中 的 
highgui API, 


这 里 我 们 会 对 src/camera timer.cpp 文 件 中 主要 部 分 的 代码 进行 解释 ， 其 中 有 一 个 实现 摄像 头 驱 动 的 类 。 它 的 属性 如 下 : 


ros: :NodeHandle nh; 
image transport::ImageTransport it; 
image transport::Publisher pub image raw; 


cv::VideoCapture camera; 
cv::Mat image; 
cv bridge::CvImagePtr frame; 


ros::Timer timer; 


int camera index; 
int fps; 


像 往常 一 样 ， 我 们 需要 节点 句柄 。 然 后 我 们 需要 用 于 以 任何 可 能 的 格式 发 送 图 像 的 ImageTransport 对 象 。 在 代码 中 ， 我 们 
只 需要 使 用 友 布 者 Publisher (唯一 的 ) ， 但 注意 它 必须 是 image transport 库 的 实现 ， 而 不 是 一 般 用 于 图 像 消 息 的 


ros::Publisher, 


然后 我 们 使 用 OpenCyV 来 捕捉 图 像 / 帧 (images/frames) 。 在 捕捉 帧 的 时 候 ， 我 们 直接 使 用 cv_brigde 帧 。 它 是 
CvlmagePtr 类 型 的 ， 通 过 它 我 们 可 以 直接 访问 图 像 字段 。 


最 后 ， 我 们 有 计时 器 和 用 于 驱动 程序 工作 的 基本 摄像 头 参 数 。 这 些 参数 包括 摄像 头 的 索引 名 ， 也 就 是 /devvideo? 设备 的 
编号 ， 例 如 ，0 代 表 /dev/video0。 这 个 索引 名 被 传递 给 cv:VideoCapture。fps 参 数 用 于 设 定 摄像 头 FPS (有 些 摄像 头 不 支持 这 
个 参数 ) 和 计时 器 。 这 里 我 们 使 用 了 一 个 int 值 ， 但 在 最 终 版 本 的 src/camera.cpp 中 它 也 可 能 是 一 个 double 值 。 


这 个 驱动 会 使 用 用 于 安 准 和 初始 化 节点 、 摄 像 头 、 计 时 器 的 类 构造 立 数 : 


nh.param<int>( "camera index", camera index, DEFAULT CAMERA INDEX ) ; 
if ( not camera.isOpened() ) 


{ 


ROS ERROR STREAM( "Failed to open camera device!" ); 
ros: :shutdown(); 


nh.param<int>( "fps", fps, DEFAULT FPS ) ; 
ros::Duration period = ros::Duration( 1. / fps ); 
pub image raw = it.advertise( "image raw", 1 ); 


frame = boost::make shared< cv bridge::CvImage >(); 
fFrame->encoding = sensor msgs::image encodings: :BGR8; 


timer = nh.createTimer( period, &CameraDriver::capture, this ); 


首先 ， 我 们 要 打开 摄像 头 ， 如 果 无 法 打开 则 需要 终止 程序 。 注 意 ， 我 们 必须 在 属性 构造 浮 效 内 完成 这 些 工 作 ， 如 下 所 示 : 
camera( camera_index ) 
这 里 camera_index 作 为 参数 进行 传递 。 然 后 ， 我 们 读 取 fps 参 数 并 计算 计时 器 周期 。 这 些 参数 用 于 创建 计时 器 并 设 定 用 于 图 


像 捕 捉 的 回调 函数 。 我 们 使 用 Image transport API 作 为 Image raw 图像 (RAW 图 像 ) 的 发 布 者 ， 并 初始 化 frame 变 量 。 图 像 捕 
捉 的 回调 函数 读 取 和 发 布 图 像 如 下 所 示 : 


camera >> frame->image; 
if( not frame->image.empty() ) 


frame->header.stamp = ros::Time::now() ; 
pub image raw.publish( frame->toImageMsg() ); 


| 
上 面 的 万 法 捕捉 图 像 ， 并 检查 帧 是 否 被 实际 捕捉 。 在 这 种 情况 下 ， 设 定时 间 截 并 友 布 已 经 转换 成 ROS Image 的 图 像 。 
使 用 以 下 代码 局 动 这 个 节点 : 


$ rosrun chapter5 tutorials camera timer camera index:=0 fps:=15 


我 们 将 会 以 15fps 的 速度 启动 /dev/video0 报 像 头 。 


然后 你 能 使 用 Image_view 或 rqt_image_view 查 看 图 像 。 和 轮 询 的 实现 方式 类 似 的 是 ， 你 有 一 个 .launch 文 件 ， 并 能 够 使 用 


以 下 命令 运行 : 


$ roslaunch chapter5 tutorials camera polling.launch camera index:=0 
fps:=15 view:=true 


现在 ， 你 能 看 到 /camera/image_raw 主 题 的 图 像 。 


在 计时 器 的 实现 方式 中 ， 最 终 的 实现 中 包含 camera.launch 文 件 ， 并 提供 更 多 的 选项 。 这 些 选 项 需要 贯穿 本 章 的 全 部 内 容 。 
在 最 终 的 实现 中 ， 最 大 的 改进 是 能 够 支持 动态 参数 再 配置 ， 也 束 是 说 ， 它 能 够 提供 包括 摄像 头 标定 在 内 的 摄像 头 信 息 。 我 们 会 简 


要 说 明 这 是 如 何 实现 的 ， 并 建议 读者 目 行 查看 源 代码 来 进一步 理解 。 


和 FireWire 报 像 头 类 似 ， 我 们 能 够 对 摄像 头 参 数 的 动态 再 配置 提供 支持 。 然 而 ， 大 多 数 的 USB 摄 像 头 并 不 支持 对 部 分 参数 的 
修改 。 我 们 所 做 的 主要 针对 所 有 OpenCV 支 持 的 参数 ， 并 在 发 生 错 误 (或 部 分 参数 失效 ) 时 向 用 户 提 示警 告 信息 。 配 置 文件 是 
cfg/Camera.cfg。 请 查看 文件 以 了 解 详细 内 容 。 它 支持 这 些 参数 : 


- camera_index: 用 于 选择 /dev/video? 设备 。 
- frame_width 和 和 frame_height: 用 于 提供 图 像 的 分 辩 率 。 


-fps: 用 于 摄像 头 的 FPS 值 。 


-fource: 表示 FOURCC 的 定义 格式 ， 用 于 指定 摄像 头像 素 (查看 http://www.fourcc.org， 虽 然 这 个 标识 符 所 指 的 格式 通常 是 
YUYVXAMJPEG, 但 对 于 大 多 数 USB 摄 像 头 它们 无 法 使 用 OpenCV 进 行 修改 ) o 


- brightness. contrast. saturation#ehue: 这 些 变量 设 定 摄像 头 的 属性 。 在 数字 摄像 头 中 ， 它 们 通过 软件 修改 ， 并 在 传感器 的 
图 像 获 取 过 程 中 进行 调整 或 直接 对 最 终 的 图 像 进 行 调 整 。 


gain: 设 定 了 传感器 模 数 转换 (ADC) 的 增益 。 它 会 向 图 像 中 引入 椒盐 噪声 ， 也 会 增加 在 黑暗 环境 中 图 像 的 亮度 。 


- exposure: 决定 了 图 像 的 曝光 ， 也 就 是 设 定 了 图 像 的 亮度 。 通 常 通过 调整 增 压 和 快门 速度 (在 廉价 摄像 头 中 ， 这 就 是 进入 
传感器 光 的 积分 时 间 ) 进行 调整 。 


-frame_id: 用 于 指定 摄像 头 坐 标 系 ， 而 且 在 导航 功能 中 这 非常 有 用 ， 同 样 我 们 在 5.6 节 也 会 用 到 它 。 
- cameta_info_url: 这 个 参数 提供 到 摄像 头 信息 的 路 径 ， 所 保存 的 内 容 主 要 关于 摄像 头 标定 。 
然后 ， 在 驱动 中 ,我们 通过 以 下 语句 使 用 动态 再 配置 服务 : 


#include <dynamic reconfigure/server.h> 


TEAS PEN PI ES Bal PAY: 


server.setCallback( boost::bind( &CameraDriver::reconfig, this, 1, 2 


) ); Oo 


回调 函数 会 再 配置 摄像 头 。 当 camera_index 改 变 时 ， 我 们 甚至 允许 改变 摄像 头 或 停止 当前 使 用 的 摄像 头 ， 然 后 使 用 
OpenCV 的 cv::VideoCapture 类 来 配置 摄像 头 属性 。 其 中 包括 了 部 分 前 面 提 到 的 参数 。 以 frame_width 参 数 为 例 说 明 : 


newconfig.frame width = setProperty( camera, CV CAP PROP FRAME WIDTH , 
newconfig.frame width ); 


它 依赖 于 前 面 提 到 的 setProperty 方 法 ， 它 会 调用 cv::VideoCapture 中 的 set 方 法 并 控制 实例 ， 在 失败 时 发 送 ROS 警 告 消息 。 
FPS 在 计时 器 中 改变 ， 而 且 在 摄像 头 中 通常 不 能 像 其 他 参数 那样 被 修改 。 最 后 最 需要 注意 的 是 ， 所 有 的 再 配置 过 程 都 受到 一 个 锁 
定 的 互 斥 标记 的 制约 ， 以 避免 在 获取 图 像 的 同时 进行 驱动 的 再 配置 。 


为 了 能 设 定 摄像 头 信 息 ，ROS 有 一 个 camera_info_ manager 库 ， 能 够 帮助 我 们 完成 这 些 工作 。 简 而 言 乙 是 使 用 : 


#include <camera_info manager/camera_info manager.h> 


我 们 用 它 来 获取 Cameralnfo 消 息 。 现 在 ， 在 计时 器 中 捕获 图 像 的 回调 消 数 里 ， 我 们 能 使 用 
image transport::CameraPublisher (不 只 是 用 于 图 像 ) 。 代 码 如 下 所 示 : 


camera >> frame->image; 
if( not frame->image.empty() ) 


frame->header.stamp = ros::Time::now(); 

*camera info = camera info manager.getCameraInfo() ; 
camera info->header = frame->header; 

camera pub.publish( frame->toImageMsg(), camera_info ); 


| 


这 会 在 前 面 提 到 的 再 配置 方法 中 所 使 用 的 互 斥 标 记 的 制约 下 工作 。 现 在 ， 我 们 为 第 一 版 驱动 进行 了 各 种 参数 配置 ， 但 在 
manager 库 中 仍然 要 使 用 再 配置 方法 (在 摄像 头 加 载 过 程 中 已 经 调用 过 一 次 ) 获取 摄像 头 信 息 ， 用 于 设 定 节点 句柄 、 摄 像 头 名 
称 和 camera_info_url 参 数 。 然 后 我 们 将 图 像 / 帧 (ROS Image) 和 Cameralmage 消 息 都 友 布 出 去 。 


运行 以 下 命令 使 用 驱动 : 


$ roslaunch chapter5 tutorials camera.launch view:=true 


Cee config/camera/webcam.yamlfFARUABSM, Big sZiBaAllaassasem, 


你 能 够 使 用 rostopic list, rostopic hz/camera/image raw 和 image view (或 rqt image view) 来 检查 摄像 头 是 否 在 工 
作 。 


我 们 使 用 了 ROs 中 所 有 可 用 的 资源 来 完成 驱动 程序 ， 以 便 进 行 摄像 头 、 图 像 己 计算 机 视觉 等 工作 。 为 了 能 讲解 得 更 清晰 ， 我 
们 将 会 在 下 面 的 小 世 中 分 别 解 释 每 一 项 工作 。 


5.3 ”标定 摄像 头 


大 部 分 摄像 头 ， 尤 其 是 广角 镜头 ， 会 引入 巨大 的 失真 。 我 们 可 以 从 径 向 和 切 向 对 这 种 失真 进行 建 模 ， 并 使 用 标定 算法 计算 模 
型 的 参数 。 这 种 摄像 头 标定 算法 同时 还 允许 我 们 获得 一 个 市 有 镜头 焦距 和 主 点 的 标定 算 阵 ， 这 就 为 我 们 提供 了 一 个 万 法 能 根据 所 
获取 的 图 像 测量 实际 环境 。 在 双 目 视 党 的 情况 下 ， 它 也 能 帮助 我 们 得 到 深度 信息 ， 也 就 是 摄像 头 获取 的 每 个 像素 之 间 的 距离 ， 我 
们 会 在 后 面 介绍 这 部 分 内 容 。 而 最 终 的 结果 束 是 我 们 获取 了 真实 环境 的 3D 信 息 ，。 


标定 是 通过 使 用 一 种 已 知 的 标定 图 案 ， 并 对 图 案 的 多 种 角度 的 视图 进行 辨识 来 实现 的 。 最 典型 的 图 案 是 棋盘 ， 也 有 以 一 列 圆 
圈 或 者 非 对 称 的 圆圈 作为 图 案 的 ， 但 要 注意 当 视 图 和 倾 斜 时 ， 圆 圈 看 起 来 成 了 椭 加 。 检 测算 法 会 获取 棋盘 上 每 个 单元 格 的 内 角 点 ， 
并 用 来 估计 摄像 头 的 内 参数 和 外 参数 。 简 单 讲 ， 外 参数 是 摄像 头 的 安 态 ， 或 者 换 句 话 襄 ， 融 是 当 摄 像 头 保持 在 某 个 固定 位 置 时 ， 
图 案 相对 于 摄像 头 的 位 次 。 我 们 最 需要 的 是 内 参数 ， 因 为 它 不 会友 生 改 变 ， 这 就 能 在 摄像 头 处 于 任何 位 次 时 使 用 ， 能 够 测量 图 像 
的 距离 ， 修 正 图 像 的 失真 ， 也 束 是 纠正 图 像 。 


运行 我 们 的 摄像 头 驱 动 ， 丈 能够 使 用 ROS 的 标定 工具 完成 摄像 头 标定 。 在 此 过 程 中 ,摄像头 驱动 提供 了 Cameralnfo 消 息 和 
camera_info_set 服 务 。 这 个 服务 允许 设 定 标定 结果 保存 的 文件 路 径 。 之 后 我 们 每 次 使 用 摄像 头 时 ， 标 定 信息 融 能 够 被 图 像 管道 


加 载 。 FireWire 摄 像 头 的 camera1394 驱 动 恰 好 满足 这 些 先 决 条 件 。 想 要 标定 你 的 FireWire 摄 像 头 ， 需 要 运行 以 下 命令 : 


$ roslaunch chapter5 tutorials calibration firewire camera chessboard. 
launch 


这 会 打开 一 个 GUI 界 面 ， 并 上 自动 选择 我 们 标定 图 案 的 视图 。 它 在 右上 角 会 显示 一 些 指示 条 来 表现 在 视图 中 的 每 一 个 “ 轴 ”， 
其 中 包括 了 x 轴 和 y 轴 ， 或 者 分 别 代 表 横 轴 和 纵 轴 。 显 示 的 这 些 轴 与 图 像 平 面 上 的 这 些 轴 表 示 了 相同 的 图 案 模 式 。 然 后 ，scale 表 
示 我 们 的 标定 泡 围 从 近 到 远 (直到 我 们 打算 进行 探测 工作 的 最 大 距离 ) 。 最 后 是 skew 显 示 视 图 中 所 识别 的 x 轴 和 y 轴 的 斜率 。 这 
些 指示 条 下 面 的 三 个 按钮 默认 是 茜 用 的 ， 如 下 页 图 上 所 示 : 


你 能 看 到 每 次 图 案 重 蔷 的 时 候 标定 算法 还 是 能 找到 这 些 点 。 这 些 视图 会 被 自动 选择 并 覆盖 一 定数 量 的 不 同 视 图 。 这 样 你 在 指 
示 的 引导 下 能 够 让 指示 条 在 某 个 时 刻 从 左 到 右 完 全 变 绿 。 理 论 上 ， 有 两 个 视图 就 足够 了 ， 但 是 实际 使 用 中 一 般 都 需要 10 个 左 
右 。 而 这 个 界面 会 捕捉 更 多 (30~40 个 ) 。 你 应 该 避免 过 快 的 移动 速度 ， 因 为 模糊 的 图 像 对 于 检测 来 说 是 非常 不 利 的 。 一 旦 这 个 
工具 收集 了 足够 的 视图 ， 它 就 会 允许 你 进行 标定 ， 也 就 是 说 根据 标定 图 案 的 视图 中 找到 的 点 对 摄像 头 模型 进行 求解 和 系统 优化 。 
如 下 页 图 下 所 示 : 
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然后 ， 你 可 以 保存 标定 数据 并 向 摄像 头 提交 标定 结果 。 这 融 需 要 使 用 camera_info_set 服 务 来 提交 。 在 后 续 使 用 中 ROS3 图 像 
管道 融会 目 动 检测 结果 


对 于 使 用 了 标定 的 launch 文 件 ， 只 需要 使 用 ROS camera calibration 功 能 包 中 的 cameracalibrator.py 文 件 ， 使 用 以 下 命 


<node pkg="camera_calibration" type="cameracalibrator.py" 
name="cameracalibrator" args="--size 8x6 --square 0.030" 
output="screen"> 


<remap from="image" to="camera/image color" /> 


<remap from="camera" to="camera" /> 
</node> 


标定 工具 只 需要 模板 的 特征 (这 里 方 格 数量 和 大 小 分 别 是 8x 6 个 和 0.030 平 方 米 ) 、 图 像 主题 、camera 的 命名 空间 。 
它 也 使 用 图 像 管道 ， 但 是 并 不 必要 。 事 实 上 ， 我 们 还 可 以 使 用 mage_raw 来 著 代 image_color 主 题 


一 旦 你 保存 了 标定 结果 (使 用 Save 按钮) ， 就 会 在 /tmp 文 件 夹 下 创建 一 个 文件 。 它 保 仔 了 标定 使 用 的 标定 图 案 的 视图 文 
件 。 你 能 够 在 /tmp/calibrationdata.tar.gz 中 找到 这 些 文件 。 在 本 书 中 用 于 标定 的 视图 文件 存储 在 calibration 文 件 夹 下 ， 其 中 
firewire_camera 子 文件 夹 是 FireWire 摄 像 头 的 第 一 个 实例 。 类 似 地 ， 在 命令 行 终端 上 (stdout 输 出 ) ， 你 能 看 到 获取 对 应 视图 
的 信息 和 标定 结果 。 本 书 获取 的 标定 结果 保存 在 同一 个 文件 夹 下 。 你 也 可 以 查看 calibrationdata.tar.gz ZIP 文 件 中 的 ost.txt 文 件 
来 查询 标定 结果 。 无 论 如 何 要 记 住 在 提交 之 后 ， 标 定 乱 阵 和 失真 模型 的 系数 会 更 新 标定 文件 。 试 验 这 个 功能 的 好 办 法 是 在 标定 之 
前 在 相应 文件 夹 下 放置 一 个 假 的 标定 文件 。 在 我 们 的 功能 包 中 ， 这 个 文件 是 


calibration/firewire camera/calibration firewire camera.Yaml， 它 由 参数 文件 访问 : 


camera info url: package://chapter5 tutorials/calibration/firewire | 
camera/calibration firewire camera.yaml 


现在 我 们 使 用 图 像 管 道 再 次 运行 我 们 的 摄像 头 。 如 果 摄 像 头 家 正确 标 定 ， 那 么 很 明显 修正 后 的 图 像 会 修正 失真 。 我 们 将 会 在 
后 面 介绍 图 像 管道 时 看 到 这 部 分 内 容 。 在 ROs 中 采用 了 OpenCV 中 实现 的 张 正 友 标 定 方法 。 如 果 有 兴趣 ， 我 们 的 建议 是 查看 相关 


文档 了 解 标 定格 式 的 更 多 细节 。 然 而 ,我 们 嘻 得 本 书 已 经 包含 了 用 尸 应 该 了 解 的 知识 


(http://docs.opencv.org/doc/tutorials/calib3d/camera calibration/camera calibration.html) 。 


最 后 ， 你 还 可 以 使 用 下 面 的 launch 文 件 试 着 应 用 不 同 的 标定 图 案 ， 如 圆 形 和 不 对 称 圆 形 ( 查 
@http://docs.opencv.org/ downloads/acircles pattern.png) ， 这 都 针对 FireWire 摄 像 头 : 


roslaunch chapter5 tutorials calibration firewire camera circles. 
launch 


roslaunch chapter5 tutorials calibration firewire camera acircles. 
launch 


$ roslaunch chapter5 tutorials camera.launch calibrate:=true 


在 下 图 中 ， 你 会 看 到 一 个 和 FireWire 报 像 头 一 样 的 GUI 中 的 标定 过 程 ， 也 就 是 说 我 们 也 要 运行 camera info set 服务。 
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Anin (CALIBRATE) 按钮 之 后 ， 标 定 优化 算法 会 得 到 最 好 的 摄像 头 内 外 参数 。 下 图 展示 了 标定 过 程 结束 后 ， 人 允许 我 们 
保存 标定 数据 并 提交 到 摄像 头 配置 文件 中 ， 而 且 我 们 不 需要 做 更 多 的 设置 。 
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双 目 标定 


在 下 面 的 章节 中 我 们 将 会 介绍 如 何 使 用 双 目 摄像 头 。 一 个 方法 是 运行 两 个 单 目 摄像 头 节 点 。 但 通 剃 情况 下 将 双 目 摄像 头 组 作 
为 一 个 传感器 来 处 理会 更 好 ， 因 为 图 像 必 须 是 同步 的 。 在 ROs 中 ， 没 有 FireWire 双 目 摄 像 头 专用 的 驱动 ， 但 是 我 们 能 够 从 下 面 找 
到 一 个 双 目 应 用 的 扩展 : 


$ git clone git://github.com/srv/cameral394stereo.git 


然而 ，FireWire 的 双 目 视觉 是 比较 昂贵 的 。 基 于 这 个 原因 ， 我 们 提供 一 个 基于 UsB 摄 像 头 的 双 目 视觉 摄像 头 驱动 。 我 们 使 用 
罗拉 的 C120USB 网 络 摄 像 头 ， 它 的 价格 相对 低廉 ， 品 声 也 很 大 ， 但 我 们 可 以 在 进行 标定 后 看 看 是 不 是 能 改善 一 下 。 保 证 双 目 视 
部 中 的 两 个 摄像 头 一 致 是 非 囊 重要 的 ， 但 是 你 也 可 以 试 着 使 用 不 同 的 摄像 头 。 我 们 安 委 的 两 个 摄像 头 如 下 图 所 示 。 你 只 需要 将 两 
个 摄像 头 安 闪 在 同一 个 标定 板 上 并 朝 同 平行 的 方 同 。 我 们 的 基线 大 约 是 12cm， 这 个 参数 在 双 目 摄像 头 标定 过 程 中 也 要 被 计算 进 
去 。 正 如 你 所 网， 你 只 需要 一 个 固定 两 个 摄像 头 的 标定 板 ， 并 用 扎 市 捆 好 : 
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成 /dewvideo0 和 /devwvideo1。 当 然 ， 如 果 0 被 占用 ， 融 是 1 和 和 2。 另外， 你 可 以 新 建 一 个 udev 规 则 。 
能 需要 有 用 的 video4linux 摄 像 头 控制 面板 ， 可 以 使 
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然后 ， 你 能 够 分 别 测试 每 个 摄像 头 ， 融 像 使 用 单一 摄像 头 时 一 样 。 你 可 
用 以 下 命令 来 安 丢 : 

$ sudo apt-get install v4l-utils qv412 

你 可 能 会 遇 到 这 样 的 错误 : 


In case of problems with stereo: 
libv412: error turning on stream: No space left on device 


之 所 以 会 友 生 这 样 的 情况 是 因为 你 应 该 把 两 个 摄像 头 分 别 连接 到 不 同 的 USB 控 制 器 上 。 请 注意 ， 有 些 USB 插 口 是 由 同一 个 控 
制 器 管理 的 。 这 样 在 连接 多 于 一 个 摄像 头 时 就 没有 足够 的 市 宽 处 理 数据 。 如 果 只 有 一 个 USB 控 制 器 ， 有 几 个 方法 你 可 以 试 一 下 。 
首先 是 在 两 个 摄像 头 上 试 着 使 用 压缩 格式 ， 例 如 MJPEG。 你 可 以 检查 你 的 摄像 头 是 否 支持 压缩 : 
$ v412-ctl -d /dev/video2 --list-formats 


你 会 看 到 类 似 如 下 的 输出 : 


ioctl: VIDIOC ENUM FMT 


Index : 0 

Type : Video Capture 
Pixel Format: 'YUYV' 

Name : YUV 4:2:2 (YUYV) 
Index ta 

Type : Video Capture 


Pixel Format: 'MJPG' (compressed) 


Name : MJPEG 


如 果 支 持 MJPEG， Fel LAE ]—TS USB les MES MaaA. REZIN, (ERASERS CURT, BIW 
的 USB 控 制 器 ， 或 者 将 分 辨 率 调 低 到 320x240 及 以 下 。 类 似 地 ， 在 qv4l2 的 GUI 界面 下 你 能 够 检查 这 个 功能 并 测试 你 的 摄像 头 。 
你 还 可 以 试 着 将 其 设置 到 理想 的 像素 格式 。 有 时 候 这 样 都 行 不 通 。 还 有 使 用 OpenCV set 万 法 对 USB 摄 像 头 来 说 并 不 起 作用 。 所 
以 我 们 还 是 在 不 同 的 USB 控 制 器 下 使 用 不 同 的 USB 插 口 。 

本 书 中 附带 的 USB 双 目 视 党 驱动 是 基于 前 面 章 节 介绍 过 的 USB 摄 像 头 驱 动 。 基 本 上 ， 它 扩展 了 对 摄像 头发 布 者 的 支持 。 发 布 
者 会 友 送 左边 和 右边 的 图 像 ， 并 附 市 摄像 头 信息 。 你 能 够 运行 它 并 使 用 以 下 命令 查看 图 像 : 


$ roslaunch chapter5 tutorials camera stereo.launch view:=true 


这 也 显示 左右 摄像 头 的 视 关 图像。 视差 图 像 是 非 负 重要 的 ， 我 们 会 在 后 面 专 门 过 论 。 一 旦 摄像 头 航 标定 好 ， 融 能 够 使 用 ROS3 
图 像 管道 。 为 了 能 够 标定 ， 我 们 需要 运行 以 下 命令 : 


$ roslaunch chapter5 tutorials camera stereo.launch calibrate:=true 


你 能 看 到 和 前 面 单 目 摄像 头 类 似 的 GUI。 


在 上 图 截屏 的 时 刻 ， 我 们 已 经 有 了 足够 的 视图 来 局 动 标定 。 注 意 标定 图 案 必须 被 两 个 摄像 头 同 时 检测 到 ， 才 能 完成 后 续 的 标 
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标定 是 由 和 单 目 摄 像 头 相同 的 cameracalibrator.py 节 点 完成 。 我 们 直接 传递 左右 摄像 头 和 图 像 ， 这 样 工 具 就 会 知道 我 们 将 
进行 双 目 标定 。 下 面 是 launch 文 件 中 的 node 字 段 : 


<node ns="$(arg camera)" name="cameracalibrator" 
pkg="camera calibration" type="cameracalibrator.py" 
args="--size 8x6 --Square 0.030" output="screen"> 
<remap from="left" to="left/image raw"/> 
<remap from="right" to="right/image raw"/> 
<remap from="left_ camera" to="left"/> 
<remap from="right camera" to="right"/> 


</node> 


标定 的 结果 和 单 目 摄 像 头 是 相同 的 ， 但 是 在 这 种 情况 下 我 们 会 分 别 保 仓 两 个 摄像 头 的 标定 文件 。 根 所 
config/camera stereo/logitech_c120.yaml 中 的 参数 文件 ， 我 们 有 : 


camera info url left: package://chapter5 tutorials/calibration/camera _ 


stereo/${NAME}.yaml 
camera info url right: package://chapter5 tutorials/calibration/ 


camera stereo/${NAME}.yaml 


这 里 ，${NAME} 是 摄像 头 的 名 称 ， 分 别 代 表 logitech_c120 left#Jlogitech c120 right， 也 就 是 左右 摄像 头 。 在 提交 标定 之 
后 ， 会 将 每 个 摄像 头 的 标定 更 新 到 这 些 文件 。 这 包含 了 标定 矩 哇 、 失 真 模 型 参数 和 透视 投影 矩阵 ， 其 中 还 包 合 基线 。 基 续 表 示 的 
是 在 图 像 平 面 x 轴 方 向 上 两 个 摄像 头 的 距离 。 在 参数 文件 中 ， 你 还 能 看 到 一 些 用 来 设 定 人 工 灯光 的 宇内 环境 的 摄像 头 属性 值 。 这 
个 摄像 头 是 目 动 校正 的 ， 所 以 有 时 候 图 像 质 量 比较 差 ， 但 大 多 数 情 况 下 还 是 可 以 接受 的 。 


~ 


54 ROS 图 像 管 道 


ROS 图 像 管 道 通过 image_proc 功 能 包 运 行 。 它 提供 了 各 种 用 于 从 摄像 头 采 集 的 RAW 图 像 中 获取 音色 和 彩色 图 像 的 转换 功 
能 。 在 使 用 FireWire 摄 像 头 的 情况 下 ， 摄 像 头 (人 在 其 图 像 传感器 中 ) 很 可 能 使 用 了 拜耳 (Bayer) 模式 进行 图 像 编 码 。 需 要 进行 


去 拜耳 化 来 获得 彩色 图 像 。 一 旦 你 标定 完 摄 像 头 ， 图 像 管 赴 融 会 提取 Cameralnfo 消 息 (其 中 包含 了 去 拜耳 化 模式 信息 ) 并 修正 
你 的 图 像 。 这 里 ， 修 正 意 味 痢 修复 图 像 ， 这 样 惑 能 用 失真 模型 的 参数 来 修正 径 向 和 切身 的 失真 。 


ROS 图 像 管 道 的 结果 是 ， 我 们 能 够 在 命名 空间 内 看 到 更 多 的 关于 摄像 头 的 主题 。 在 下 图 中 ， 你 能 看 到 主题 image_raw、 
image_mono 和 image_color 分 别 显 示 了 RAW、 单 色 图 像 和 彩色 图 像 


enrique@siani: ~ ©@® /camera/image_raw 
roscore http://siani... x /home/enrique/ros... x | enrique@siani: ~ 
/camera/image _rect/theora/parameter_updates 
/camera/image rect _color 
/camera/image rect_color/compressed 
/camera/image rect_color/compressed/parameter_descriptions 
/camera/image rect _color/compressed/parameter_ updates 
/camera/image rect_color/compressedDepth 
/camera/image rect_color/compressedDepth/parameter_descriptio 
ns 
/camera/image rect_color/compressedDepth/parameter_updates 
/camera/image rect_color/theora 
/camera/image rect_color/theora/parameter_descriptions 
/camera/image rect_color/theora/parameter_updates 
/camerai1394_node/parameter_descriptions 
/camerai394_node/parameter_updates 


enrique@stani:~$ rosrun image view image view image:=/camere 
/image_raw 

senrique@siani:~$ rosrun image view image view image:=/camera 
/image mono 

enrique@siani:~$ rosrun image view image view image:=/camera 
image _ raw 


内 月 日 /camera/image_mono 


roscorehttp://siani... x /home/enrique/ros... x - enrique@siani: ~ 


/camera/image_rect_color/compressed 

/camera/image rect _color/compressed/parameter_ descriptions 
/camera/image rect_color/compressed/parameter_updates 
/camera/image rect_color/compressedDepth 
/camera/image_rect_color/compressedDepth/parameter_descriptio 
ns 

/camera/image _rect_color/compressedDepth/parameter_updates 
/camera/image_rect_color/theora 
/camera/image_rect_color/theora/parameter_descriptions 
/camera/image_rect_color/theora/parameter_updates 
/camerai394_ node/parameter_descriptions 

/camerai394_ node/parameter_updates 

/diagnostics 

/rosout 

/rosout_agg 

enrique@siani:~$ rosrun image view image view image:=/camera 
/image raw 

enrique@siani:~$ rosrun image view image view image:=/camera 
/image mono 

enrique@siani:~$ rosrun image view image view image:=/camera 
/image_raw 

enrique@siani:~$ rosrun image view image view image:=/camera 
image_mono 


enrique@siani: ~ 四 月 日 /camera/image_ color 


roscore http://siani... x | /home/enrique/ros... x | enrique@siani: ~ 


La 
/camera/image rect color/compressed/parameter updates 


/camera/image rect color/compressedDepth 

/camera/image rect color/compressedDepth/parameter descriptio 
ns 

/camera/image_rect_color/compressedDepth/parameter_updates 
/camera/image_rect_color/theora 

/camera/image _rect_color/theora/parameter_descriptions 
/camera/image_rect_color/theora/parameter_updates 

/camerai394 node/parameter_ descriptions 


rosrun image view image view image: 
rosrun image view image view image: 
rosrun image view image view image: 
rosrun image view image view image: 


enrique@siant: rosrun image _ view image_view image: 
image_color 


在 主题 image_rect 和 image _rect_color 中 ， 分 别提 供 修正 后 的 音色 和 彩色 图 像 。 人 在 下 图 中 ， 我 们 比较 未 标定 过 的 、 扭 曲 的 
源 图 像 和 修正 后 的 图 像 。 你 能 分 辨 出 哪个 是 修正 的 是 因为 在 下 图 的 图 案 中 ， 只 有 修正 后 的 才 有 直线 。 尤 其 是 在 远离 图 像 中 心 ( 传 
感 嚣 的 原点 ) 的 区 域 里 : 


/camera/image color 


\ 


OO /camera/image rect color 


你 能 通过 rostopic list 或 rqt_graph 命 令 查 看 所 有 活动 的 主题 ， 这 也 包括 image transport 主题 。 你 能 直接 得 看 单 目 摄像 头 的 
image_raw 主 题 ， 使 用 以 下 命令 : 


$ roslaunch chapter5 tutorials camera.launch view:=true 
它 也 能 用 来 查看 其 他 的 主题 。 但 是 对 于 一 些 摄像 头 ，RAW 图 像 已 经 是 彩色 的 了 。 如 果 要 查看 修正 后 的 图 像 ， 也 可 以 用 


image view 或 者 rqt image _view 查 看 image _rect_color 主 题 ， 或 者 修改 launch 文 件 。 通 过 image_proc 节 点 可 以 激活 这 些 主 
题 ， 只 需要 在 launch 文 件 中 加 入 以 下 代码 : 


<node ns="s (arg camera)" pkg="image proc" type="image proc" 
name="image proc"/> 


es oT eS 
一 JO & = HE] 


在 双 目 摄像 头 的 情况 下 ， 左 右 摄像 头 都 是 相同 的 。 然 而 ， 也 有 一 些 专门 针对 它们 开发 的 可 视 化 工具 ， 这 样 我 们 就 能 够 使 用 左 
石 图 像 来 计算 和 查看 视差 图 像 。 其 主要 算法 是 使 用 双 目 视 党 标定 和 左右 图 像 纹理 估计 每 个 像 夫 的 深度 ， 也 就 是 视差 图 像 。 为 了 获 
得 优秀 的 计算 结果 ， 必 须 调整 计算 这 种 图 像 的 算法 。 在 下 图 中 我 们 看 到 左 、 右 和 视差 图 像 ， 也 就 是 使 用 stereo_image _proc 的 
rqt_reconfiguire， 在 其 中 节操 为 双 目 图 像 建 六 了 图 像 管 道 。 在 launch 文 件 中 我 们 只 需要 : 


<node ns="$ (arg camera)" pkg="sStereo image proc" type="stereo image __ 
prog“ 
name="stereo image proc" output="screen"> 
<rosparam file="$(arg params disparity)"/> 
</node> 


如 下 图 所 示 ， 它 需要 能 够 设 定 rqt_reconfigure 的 视差 参数 ， 并 与 rosparam dump/stereo/stereo image proc 一 起 保存 : 


Recon! File = @) 2043 $ Enrique Ù 


enr (\quefipc-acer:~/dev/rosbook/chapteré tutorta 
t fg do Make 
CMakeLists.txt Launch mani 


í t ! ] mainpage. dox 
cnr quegpc - acer: ~/dev/rosbook/chapteré_tutortal 
figure reconfigure_gut 

reconfigure gui started 


AAA Reconfigure 
/stereo/stereo_image_ proc 
prefilter_size: > G) Siig 
prefitter_cap: 1 Q} —— 63 | 
correlation_window_size: > =.) 255 |33 
min_disparity: “128 — 128 25 
disparity_range: 32 © 128 [32 [ 
uniqueness_ratio: © G) 100 |s 
texture_threshold: 0 «))— -10000 [3000 
speckle_ size:0 Q) 1000 [s0 | 
speckle_range: 0 «= ) 31h 45 


我 们 有 一 套 调 整 好 并 适用 于 本 书 编写 时 环境 的 参数 ， 保 存在 参数 文件 config/camera_stereo/disparity.yaml 中 : 


{correlation window size: 33, disparity range: 32, min disparity: 25, 
prefilter cap: 5, 

prefilter size: 15, speckle range: 15, speckle size: 50, texture 
threshold: 1000, 


uniqueness ratio: 5.0} 


然而 ， 这 些 参数 很 大 程度 上 依赖 于 标定 质量 和 环境 。 你 应 该 调整 它 来 适合 目 己 使 用 的 环境 。 这 是 很 费时 间 的 ， 有 时 候 也 很 未 
手 。 但 是 你 可 以 参考 以 下 的 指导 进行 。 基 本 上 ， 你 会 从 设 定 disparity_range 的 值 开 始 ， 这 个 值 允许 一 定 范围 的 视差 色 块 。 你 还 
需要 设 定 min_ disparity， 以 便 看 到 履 盖 一 整 片区 域 的 深度 显示 (从 红色 到 监 色 /紫色 ) 。 然 后 ， 你 要 调整 好 speckle_ size 来 消除 
小 的 噪声 斑点 ， 并 通过 修改 unidqueness ratio 和 texture threshold 来 获得 较 大 的 视 疾 色 块 。 参 数 correlation window size 也 会 
影响 初始 视差 色 块 的 检测 。 


如 果 忆 是 很 难 获得 好 的 检测 结果 ， 那 么 你 也 许 不 得 不 进行 多 次 标定 ,或 者 根据 你 的 环境 和 灯光 条 件 换 一 套 更 好 的 摄像 涉 ， 你 
也 可 以 试 着 在 其 他 的 环境 中 进行 实验 或 者 多 后 几 个 灯 。 你 最 好 找 一 个 纹理 比较 多 的 环境 ， 因 为 站 在 一 个 日 墙 前 面 你 是 很 难 找 到 任 
何 视 舌 的 。 还 有 由 于 基线 的 原因 ， 你 不 能 获取 与 摄像 头 特别 近 的 深度 信息 。 对 于 双 目 视 党 导航 ， 最 好 有 一 个 长 基线 ,一般 而 言 是 
12cm 或 更 远 。 这 里 用 到 它 是 因为 之 后 我 们 将 尝试 视 完 里 程 计 。 然 而 ， 使 用 这 种 配置 ， 我 们 只 有 距离 摄像 涉 1 米 的 深度 信息 。 如 
果 选 择 更 小 的 基线 ， 那 么 束 能 够 获取 更 近 物 体 的 深度 信息 。 这 不 适 于 导航 ， 因 为 无 法 找到 较 远 一 点 路 径 的 解决 万 案 ， 但 却 对 精确 
观察 和 抓 取 物体 很 有 利 。 


对 于 标定 问题 ， 你 能 够 通过 cameracheck.py 节 点 检查 标定 结果 ， 它 已 经 集成 在 了 单 目 和 双 目 摄像 头 的 launch 文 件 中 : 


$ roslaunch chapter5 tutorials camera.launch view:=true check:=true 


$ roslaunch chapter5 tutorials camera stereo.launch view:=true 
check: =true 


对 于 单 目 摄像 头 ， 我 们 的 标定 会 产生 均 方 根 (RMS) 误差 (通过 calibration/camera/cameracheck-stdout.log 查 看 更 多 


SR): 


Linearity RMS Error: 1.319 Pixels Reprojection RMS Error: 1.278 
Pixels 
Linearity RMS Error: 1.542 Pixels Reprojection RMS Error: 1.368 
Pixels 
Linearity RMS Error: 1.437 Pixels Reprojection RMS Error: 1.112 
Pixels 
Linearity RMS Error: 1.455 Pixels Reprojection RMS Error: 1.035 
Pixels 
Linearity RMS Error: 2.210 Pixels Reprojection RMS Error: 1.584 
Pixels 
Linearity RMS Error: 2.604 Pixels Reprojection RMS Error: 2.286 
Pixels 
Linearity RMS Error: 0.611 Pixels Reprojection RMS Error: 0.349 
Pixels 


对 于 双 目 摄像 头 ， 我 们 产生 极 线 误差 (epipolar error) 和 标定 图 案 中 单元 格 大 小 的 估计 (查看 


calibration/camera stereo/cameracheck-stdout.log 文 件 ) : 


epipolar error: 0.738753 pixels dimension: 0.033301 m 
epipolar error: 1.145886 pixels dimension: 0.033356 m 
epipolar error: 1.810118 pixels dimension: 0.033636 m 
epipolar error: 2.071419 pixels dimension: 0.033772 m 
epipolar error: 2.193602 pixels dimension: 0.033635 m 
epipolar error: 2.822543 pixels dimension: 0.033535 m 


为 了 获取 这 些 结果 ， 你 只 需要 向 摄像 头 提交 标定 图 案 。 这 也 是 我 们 会 传递 view : =true 到 launch 文 件 的 原因 。 一 般 来 说 均 方 
根 误差 如 果 大 于 2 个 像素 ， 那 么 殉 是 比较 大 的 误差 了 。 即 使 如 此 我 们 还 是 能 够 在 这 个 条 件 下 进行 一 些 实 验 ， 因 为 你 也 要 记 住 这 些 
都 是 廉价 的 摄像 头 。 当 然 如 果 你 的 误差 小 于 1 个 像素 ， 那 就 非常 理想 了 。 对 于 双 目 摄像 头 ， 极 线 误 差 应 该 低 于 1 个 像素 。 但 在 我 
们 的 示例 中 ， 它 还 是 非常 大 的 (有 时 候 甚 至 高 于 3 个 像素 ) ， 即 使 如 此 它 也 是 可 用 的 。 事 实 上 ， 视 差 图 像 残 是 通过 stereo_view 
三 点 显示 和 表示 每 个 像素 的 深度 信息 。 我 们 还 有 在 rviz 中 能 够 查看 纹理 的 3D 点 云 。 我 们 会 使 用 这 个 工具 做 视 党 里 程 计 。 


5.5 “计算 机 人 视 锅 任务 中 有 用 的 ROS 功 能 


在 ROs 中 使 用 计算 机 视觉 最 为 有 利 的 条 件 是 我 们 不 需要 重复 开 友 。 我 们 可 以 使 用 大 量 的 第 三 方 软件 ， 也 可 以 将 视 党 设备 与 真 
实 的 机 器 人 相连 ,或 者 仅仅 做 仿真 。 这 里 ， 我 们 将 会 列举 一 些 有 趣 的 计算 机 视 嘻 工具。 它们 能 够 应 用 于 大 量 通 用 的 视 完 任务 。 但 
是 我 们 只 会 在 后 面 详细 介绍 其 中 的 一 个 (包括 安 六 和 使 用 的 所 有 步骤 ) 。 这 融 是 视觉 里 程 计 (visual odometry) ， 当 然 其 他 的 
功能 包 也 非常 容易 安 闪 和 使 用 。 只 需要 跟着 超 链 接 中 的 教程 或 手册 进行 学 习 即 可 。 具 体 的 列表 如 下 : 


Visual Servoing (也 被 称 为 Vision-based Robot Control) : 这 是 一 种 使 用 从 视觉 传感器 获取 的 反馈 信息 来 控制 机 器 人 动作 的 


R, AFR AFAR MR. ZEROS # RAIT VA Jl Visual Servoing Platform (ViSP) 软件 控制 夹 持 器 。 

(http://www. itisa.fr/lagadic/visp/visp.htmlfehttp: //www.tos.org/wiki/vision_visp) 。ViSP 是 一 种 完全 跨 平台 的 库 ， 能 够 在 视觉 追 

宗 和 视觉 伺服 领域 进行 原型 设计 和 应 用 开发 。ROS 软 件 包 提 供 了 一 种 跟踪 器 ， 它 能 够 通过 visp_tracker (移动 边缘 跟踪 器 ) 节点 或 
visp_auto_tracker (基于 模型 的 跟踪 器 ) 节点 运行 。 它 还 能 帮助 摄像 头 标 定 和 手眼 标定 。 手 眼 标 定 对 于 抓 取 任 务 中 的 视觉 伺服 是 极 
为 重要 的 。 


- Augmented Reality (AR) : 一 种 通过 在 真实 环境 中 履 盖 虚拟 图 像 实 现 的 增强 现实 应 用 。 实 现 这 一 功能 的 最 著名 的 库 是 
ARToolkit (http://www.hitl.washington.edu/artoolkit/) 。 这 个 应 用 的 主要 问题 在 于 跟踪 用 户 的 视点 ， 所 以 虚拟 图 像 要 画 到 用 户 在 
真实 环境 中 所 关注 的 视点 上 。ARToolkit 视 频 追 踪 库 会 计算 实际 摄像 头 的 位 置 和 方向 并 进行 实时 的 物理 标记 。 在 ROS 中 我 们 有 一 
个 名 为 af_pose (http://www.tos.org/wiki/ar_pose) 的 软件 包 。 它 允许 我 们 追踪 一 个 或 多 个 标记 ， 并 在 相应 位 置 呈 现 我 们 的 虚拟 图 
ik (例如 ，3DD 模 型 ) 。 


- Perception and object recognition: 使 用 OpenCV 库 就 能 完成 最 基本 的 感知 与 物体 识别 。 当 然 ， 也 有 一 些 功 能 包 提 供 物 体 识别 
管道 ， 例 如 object_recognition 提 供 tabletop_object_detector 识 别 桌 子 上 的 物体 ， 更 通用 的 解决 方案 是 Object Recognition 
Kitchen (ORK) 可 以 参考 网 址 http://we-perception.github.io/object_recognition_core。 然 而 还 有 一 个 更 有 价值 去 探索 的 工具 就 是 
RoboEarth (http://www.roboearth.org) 。 它 允许 我 们 对 物体 进行 探测 和 3D 建 模 ， 并 将 其 存储 在 一 个 全 球 性 的 数据 库 中 。 这 个 数据 
库 允 许 全 世界 所 有 机 器 人 (RA) 访问 。 存 储 模 型 可 以 是 2D 或 3D 的 ， 它 可 以 用 于 识别 相似 的 物体 或 它们 的 视点 ， 也 就 是 说 识别 
摄像 头 / 机 器 人 看 到 的 物体 。RoboEarth (机 器 人 地 球 ) 计划 已 经 被 集成 在 ROS 中 ， 而 且 已 经 有 很 多 个 教程 供 系 统 运 行 和 试用 
(http://www.ros.org/wiki/roboearth) o 


- Visual odometty: 视 沉 里 程 计 章法 会 假定 在 一 个 静止 的 环境 中 使 用 环境 图 像 来 跟踪 特征 并 估计 机 器 人 的 运动 。 它 使 用 单 目 

或 双 目 视觉 系统 能 够 解决 机 器 人 6 自由 度 的 定位 问题 。 当 然 在 单 目 视觉 系统 下 还 需要 一 些 附 加 信息 。 视 觉 里 程 计 可 以 使 用 两 个 算 
法 库 : libviso2 (http://www.cvlibs.net/software/libviso2.html) 和 libfovis (http://www.tos.org/wiki/fovis_tos) ， 这 两 者 都 是 ROS 软 
件 包 的 一 部 分 。 你 可 以 直接 在 ROS 中 找到 ， 它 们 分 别 是 viso2 和 fovis。 在 下 面 的 小 节 中 ， 我 们 会 使 用 自制 的 双 目 摄像 头 调用 viso2 
中 的 viso2_ros 节 点 实现 视觉 里 程 计 。 库 libviso2 允 许 我 们 完成 单 目 或 双 目 视觉 里 程 计 ,， 但 是 对 于 单 目 里 程 计 来 说 ， 还 需要 进行 地 板 
平面 的 位 置 估计 。 你 可 以 用 IMU ( 见 第 4 章 ) 来 试 一 下 单一 摄像 头 的 单 目 里 程 计 ， 但 是 如 果 用 双 目 摄像 头 总 能 得 到 更 好 的 结果 和 
更 正确 的 标定 ， 正 如 本 章 前 几 节 所 介绍 的 。 最 后 ，libfovis 库 并 不 支持 单 目 视觉 ， 但 是 它 支 持 RGBD 摄 像 头 ， 例 如 Kinect 传 感 器 

( 见 第 6 章 ) 。 对 于 使 用 双 目 视觉 来 说 ， 你 最 好 试 一 下 这 两 个 库 ， 并 看 哪个 库 在 你 的 环境 中 性 能 更 好 。 这 里 ， 我 们 为 大 家 提供 一 


个 在 ROS 中 从 安装 到 运行 viso2 和 fovis 库 ， 并 支持 Kinect 的 详细 教程 。 


5.6 ”使 用 viso2 实 现 视 名 里 程 计 


为 了 使 用 viso2， 你 需要 切换 到 catkin 工 作 空间 间 目 录 下 (~/dev/catkin ws) 并 使 用 如 下 命令 : 


$ cd src 


$ wstool init 


$ wstool set viso2 --git git://github.com/srv/viso2.git 


$ wstool update 


S G0 a 


$ catkin make 


一 旦 编译 完成 ， 我 们 需要 使 用 下 面 命令 设置 环境 变量 : 


$ source devel/setup.bash 


设 定好 开 友 环境 变量 ， 就 能 够 运行 viso2 ros 节 点 ， 例 如 将 要 使 用 的 stereo odometer。 在 此 之 有 前， 还 需要 发 布 摄像 头 和 机 
器 人 (或 机 器 人 基本 连接 ) 之 间 的 坐标 变换 。 双 目 摄 像 头 驱动 已 经 安装 好 ， 我 们 会 在 后 面 的 小 节 介 绍 它 是 如 何 工作 的 。 


5.7 ”使 用 RGBD 深 度 报 像 头 实现 视 克 里 程 计 


现在 我 们 学 习 如 何 使 用 RGBD 深 硫 摄 像 头 和 fovis 软 件 实现 视 锅 里程 计 。 


5.8 ”计算 两 幅 图 像 的 蛙 应 性 


单 应 性 窍 阵 (homography matrix) 是 一 个 3x 3 算 阵 ， 它 提供 从 一 个 给 定 的 图 像 到 一 个 新 图 像 的 共 面 转换 。 在 示例 
src/homography.cpp 中 ， 摄 像 头 采 集 第 一 帧 图 片 ， 然 后 通过 它 对 比 新 采集 的 每 一 帧 图 片 并 计算 单 应 性 。 这 个 示例 首先 需要 一 些 
平 的 物体 ， 比 如 一 本 书 的 封面 ， 然 后 运行 以 下 命令 : 


$ roslaunch chapter5 tutorials homography. launch 


摄像 头 驱动 程序 从 摄像 头 启动 并 采集 帧 ， 检 测 特 征 (默认 SURF) ， 提 取 描 述 符 ， 使 用 Flann-based 交 叉 确认 过 滤器 
(cross-check filter) 对 比 第 一 帧 图 像 提 取 匹 配 信息 。 一 旦 程序 匹配 ， 计 算 单 应 性 矩阵 H。 使 用 H， 我 们 可 以 将 新 一 帧 的 图 像 变 
形 至 原始 帧 ， 如 下 面 截图 所 示 (顶部 是 匹配 点 和 通过 H 变 形 后 的 图 像 ， 终 端 中 是 相应 的 文本 显示 ) : 


本 章 对 ROS 提 供 的 计算 机 视 锅 工具 进行 了 一 个 概述 。 我 们 从 介绍 如 何 连接 和 运行 多 种 摄像 头 开始 讲 起 ， 尤 其 是 FireWire 和 
导 


UsB 摄 像 头 。 然 后 介绍 了 改变 摄像 头 参数 的 基本 功能 ， 以 便于 你 能 够 通过 调整 参数 获得 更 局 质量 的 图 像 。 除 此 之 外 ， 我 们 还 提供 


了 一 个 完整 的 USB 摄 像 头 驱动 示例 。 


然后 ， 我 们 开始 介绍 摄像 头 标 定 。 在 这 之 中 你 学 习 到 标定 一 个 摄像 头 是 多 么 简单 。 进 行 标定 的 重要 性 在 于 它 能 够 纠正 广角 镜 
头 的 失真 现象 ， 尤 其 对 于 廉价 的 摄像 涉 。 还 有 ， 标 定 矩 阵 允 许 你 进行 多 种 计算 机 视 党 任务 ， 例 如 视 完 里 程 计 和 感知 。 


我 们 展示 了 如 何在 ROs 中 使 用 双 目 视 竞 ， 如 何 安装 和 使 用 两 个 廉价 摄像 头 ， 还 解释 了 图 像 营 道 和 ROSs 中 计算 机 视 殴 相 天 的 几 
个 API， 例 如 cv bridge, image transport 和 ROS 功 能 包 中 集成 的 OpenCV 库 。 


最 后 ， 我 们 列举 了 RO3s 文 持 的 一 些 计算 机 视 营 工具 及 这 些 工具 的 应 用 方向 。 特 别 地 ， 我 们 介绍 了 使 用 viso2 和 fovis 库 完成 视 
部 里 程 计 的 示例 ， 展 示 了 一 些 使 用 高 质量 摄像 头 记 录 数 据 的 示例 和 使 用 廉价 摄像 头 的 示例 。 最 后 ， 展 示 了 如 何 使 用 特征 检测 、 摘 
述 符 提 取 和 匹配 获取 两 张 图 像 乙 间 的 单 应 性 。 因 此 ， 在 学 习 完 本 章 并 运行 示例 代码 之 后 ， 你 残 可 以 在 ROs 中 开始 计算 机 视觉 的 使 
用 和 人 研究 了 。 


在 下 一 章 ， 你 将 学 习 使 用 PCL 处 理 点 云 (point cloud) ， 这 样 就 可 以 运用 RGBD 摄 像 头 了 。 


NaeAFHILAA, Rae — Tae BUR AE DERRE RRRA, Meee SITAE (Time of 
Flight) SRAM BOHM. AKERE DISER PISIATAIR SERA. RA (PCL) 提供 了 大 量 数 
据 类 型 和 数据 结构 ， 不 仪 能 够 方便 地 表示 采样 空间 中 的 点 ， 而 且 可 以 表示 采样 空间 的 不 同属 性 ， 比 如 颜色 、 法 向 量 等 。PCL 同 样 
提供 了 许多 最 先进 的 算法 对 数据 样本 进行 处 理 ， 比 如 滤波 、 模 型 估计 、 表 面 重 建 等 。 


ROS 提 供 了 一 种 基于 消息 的 接口 (PCL 点 云 可 以 通过 该 接口 进行 有 效 的 通信 ) ， 还 有 一 组 将 本 地 的 PCL 类 型 转换 到 ROS 消 息 
的 转换 函数 ， 这 和 处 理 OpenCV 图 像 一 样 。 除 了 ROS API 的 标准 冰 数 之 外 ， 还 有 一 些 标准 的 功能 包 可 以 用 来 与 常见 的 3D 传 感 器 
进行 交互 ， 比 如 广泛 运用 的 微软 的 Kinect 或 者 Hokuyo 的 激光 功能 ， 并 且 可 以 在 rviz 的 不 同 参 考 坐 标 系 下 实现 数据 可 视 化 。 


本 草 首 先 介 绍 PCL 库 的 背景 、 相 关 的 数据 类 型 ,以 及 ROS 接 口 消息 ， 然 后 展示 一 些 关 于 如 何 使 用 PCL 库 处 理 数据 以 及 如 何 通 
过 ROS 友 送 和 接收 数据 的 技术 。 


6.1 理解 点 云 库 


在 研究 代码 之 前 ， 理 解 点 云 库 和 ROS 的 PCL 接 口 的 基本 概念 很 重要 。 束 像 前 面 提 到 的 ， 前 者 为 处 理 3D 数 据 提供 了 一 组 数据 
结构 和 算法 ， 后 者 提供 了 一 组 消息 以 及 消息 与 PCL 数 据 结构 之 间 的 转换 尔 数 。 所 有 这 些 软 件 功 能 包 和 库 ， 青 结合 ROS 提 供 的 分 布 
式 通信 层 的 能 力 ， 扩 展 了 机 器 人 领域 的 众多 新 应 用 |。 


概 言 之 ，PCL 包 含 了 一 个 非常 重要 的 数据 结构 ， 那 束 是 点 云 。 这 个 数据 结构 被 设计 成 一 个 模板 类 ， 它 把 点 的 类 型 当 作 模 板 类 
的 参数 。 因 此 ， 操 云 类 实际 上 是 一 个 点 容器 ， 这 个 容器 里 包含 了 所 有 操 云 需要 的 公共 信息 ， 而 不 管 上 各 是 什么 类 型 。 下 面 是 点 云 中 


最 重要 的 公共 字段 : 
‘header: 这 个 字段 是 pcl::PCLHeader 类 型 ， 指 定 了 点 云 的 获取 时 间 。 
points: 这 个 字段 是 std::vector 类 型 ， 它 是 储存 所 有 点 的 容器 。vector 定 义 中 的 PointT 对 应 于 类 的 模板 参数 ， 即 点 的 类 型 。 
Width: 这 个 字段 指定 了 点 云 组 织 成 一 种 图 像 时 的 宽度 ， 否 则 它 包 含 的 是 云 中 点 的 数量 。 
‘height: 这 个 字段 指定 了 点 云 组 织 成 一 种 图 像 时 的 高 度 ， 否 则 它 总 是 1。 
-is dense: 这 个 字段 指定 了 点 云 中 是 否 有 无 效 值 (无 穷 大 或 NaN 值 ) o 
sensor_otigin_: 这 个 字段 是 Eigen::Vectot4f 类 型 ， 并 且 定 义 了 传感器 根据 相对 于 原点 的 平移 所 得 到 的 位 姿 。 
sensor_otientation_: 这 个 字段 是 Eigen::Quatetnionf 类 型 ， 并 且 定 义 了 传感器 旋转 所 得 到 的 位 姿 。 


PCL 算 法 利用 这 些 字段 来 处 理 数据 ， 并 且 用 尸 可 以 利用 它们 来 创造 目 己 的 算法 。 一 旦 明日 了 点 云 的 结构 ， 下 一 步 束 是 理解 一 
个 点 云 可 以 包含 不 同 的 点 类 型 、PCL 如 何 工作 以 及 ROS 中 的 PCL 接 口 。 


6.2 我 的 第 一 个 PCL 程 序 


本 节 将 学 习 如 何 集成 PCL 和 ROS。 非 常 有 必要 知道 并 理解 ROS 软 件 包 如 何 布局 以 及 如 何 编译 ， 尽 管 这 些 步骤 是 简单 的 重复 。 
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第 一 步 是 在 你 的 工作 空间 为 本 章 创建 一 个 ROSS 软件 包 。 这 个 软件 包 依赖 于 pcl_conversions、pcl_ros、pcl_msgs 和 


sensor msgs 包 : 


$ catkin create pkg chapter6 tutorials pcl conversions pcl ros pcl msgs 
sensor msgs 


下 一 步 是 用 下 面 的 命令 在 软件 包 中 创建 一 个 源 文件 目录 : 


$ rospack profile 
$ roscd chapter6 tutorials 


$ mkdir src 


在 新 的 源 文件 目录 中 ， 应 该 创建 一 个 名 为 pcl_sample.cpp 的 文件 ， 并 输入 以 下 代码 。 它 可 以 创建 一 个 ROS 世 点 并 且 友 布 一 
个 市 有 100 个 元 素 的 点 云 。 再 次 说 明 ， 不 用 关心 这 些 代码 会 做 什么 ， 因 为 它 的 目的 只 是 实现 一 个 能 够 有 效 使 用 PCL 并 且 编 译 没有 


[RATT FA: 


#include <ros/ros.h> 

#include <pcl/point cloud.h> 

#include <pcl ros/point_ cloud.h> 

#include <pcl conversions/pcl conversions.h> 
#include <sensor msgs/PointCloud2.h> 


main (int argc, char** argv) 

{ 
ros::init (argc, argv, "pcl_ sample") ; 
ros::NodeHandle nh; 


ros::Publisher pcl pub = nh.advertise<sensor msgs: :PointCloud2> 
("pel Output", Tjj 


sensor msgs::PointCloud2 output; 


pcl::PointCloud<pcl: :PointXYZ>::Ptr cloud (new pcl::PointCloud<pc 
1::PointXYZ>) ; 


// Fill in the cloud data 
cloud->width = 100; 
cloud->height = 1; 


cloud->points.resize (cloud->width * cloud->height) ; 


//Convert the cloud to ROS message 
pcl::toROSMsg (*cloud, output) ; 


pel pub.publish(output) ; 
ros::spinOnce() ; 


return 0; 


下 一 步 是 添加 PCL 库 到 CMakeLists.txt， 这 样 可 执行 的 ROS 节 点 就 可 以 正确 地 链接 到 系统 的 PCL 库 。 


find package (PCL REQUIRED) 


include directories(include ${PCL INCLUDE DIRS}) 
link directories (${PCL LIBRARY DIRS}) 


最 后 ， 添 加 产生 可 执行 程序 和 链接 到 合适 的 库 的 代码 : 


add executable(Pcl sample Src/pcl sample.cpp) 
target link libraries(pcl_ sample ${catkin LIBRARIES} ${PCL LIBRARIES}) 


一 旦 到 了 最 后 一 个 步骤 ， 功 能 包 可 以 通过 在 工作 空间 的 根 目 录 调 用 catkin_make 来 编译 。 


6.3 IEI 


分 割 是 将 数据 组 分 割 为 满足 特定 标准 的 不 同 数据 块 的 过 程 。 分 割 可 以 通过 许多 不 同 的 方法 按照 不 同 的 标准 实现 。 有 时 候 ， 也 
许 会 涉及 从 一 个 点 云 中 基于 统计 特性 提取 结构 化 信息 ， 而 在 其 他 情况 下 ， 也 可 能 仪 仪 需要 提取 指定 颜色 泄 围 内 的 点 。 


在 许多 情况 下 ， 我 们 的 数据 可 能 满足 一 个 特定 的 数学 模型 ， 例 如 一 个 平面 、 一 条 直线 ， 或 是 一 个 球体 ， 以 及 其 他 模型 。 在 这 
种 情况 下 ， 可 以 使 用 模型 估计 算法 计算 与 数据 相 匹 配 的 模型 参数 。 有 了 这 些 参数 ， 束 有 可 能 提取 出 属于 这 个 模型 的 点 并 评估 它们 
的 匹配 度 。 


在 本 节 示 例 中 ， 我 们 将 展现 如 何 执行 一 个 基于 模型 的 点 云 分 割 。 我 们 准备 将 自己 限定 到 一 个 平面 模型 中 ， 它 是 最 常用 的 数学 
模型 ， 通 常 可 以 匹配 一 个 点 云 ， 对 于 这 个 示例 ， 我 们 也 会 执行 模型 估计 中 广泛 流行 的 随机 抽样 一 致 (RANdom SAmple 
Consensus, RANSAC) 算法 ， 它 是 一 个 和 迭代 算法 ， 即 使 存在 离 群 值 也 可 以 进行 准确 的 估计 。 


这 个 示例 可 以 在 chapter6 tutorials 功 能 包 中 找到 ， 文 件 名 为 pc|l planar segmentation.cpp: 


#include <ros/ros.h> 

#include <pcl/point cloud.h> 

#include <pcl conversions/pcl conversions.h> 
#include <pcl/ModelCoefficients.h> 

#include <pcl/sample consensus/method types.h> 
#include <pcl/sample consensus/model types.h> 


#include <pcl/segmentation/sac_ segmentation.h> 
#inelude <«pel/filters/extract_indices.hs> 
finclude <sensor_msgs/PointCloud2.h> 


class cloudHandler 

{ 

public: 
cloudHandler () 
{ 


pel sub = nh.subscribe("pcl downsampled", 10, 
EcloudHandler::cloudCB, this) ; 


pel pub = nh.advertise<sensor msgs: :PointCloud2=>("pcl_ 
segmented", 1); 

ind pub = nh.advertise<pcl msgs::PointIndices> ("point _ 
indices", 1); 

coef pub = nh.advertise<pel msgs: :ModelCoefficients=>= ("plana 
r coef", 1); 


} 


void cloudCB(const sensor _msgs::PointCloud2 &input) 
{ 

pel: :BointCloud<pel: :BPointxXYZs cloud; 

pel: :PointCloudepel: :PointxyYyg> cloud segmented; 


pel::fromROSMsg(input, cloud); 


pel: :ModelCoefficients coefficients; 
pel::PointIndices::Ptr inliers (new pel: :PointIndices()); 


pel: :SACSegmentation<pcl::PointxXYZ> segmentation; 
seqmentation.setModelType (pcl::SACMODEL PLANE} ; 
segmentation. setMethodType (pcl::SAC RANSAC) ; 
seqmentation.setMaxIterations (1000) ; 
segmentation. setDistanceThreshold(0.01); 
segmentation, seétInputCloud(cloud.makeShared{)}; 
seqmentation.seqment (*inliers, coefficients); 


pel msqs::ModelCoefficients ros coefficients; 

pel conversions: :fromPCL(coefficients, ros coefficients) ; 
ros coefficients .header.stamp = input.header. stamp; 

coef pub.publish(ros_ coefficients) ; 


pel msqs::PointIndices ros _inliers; 

pel conversions: :fromPChL(*inliers, ros inliers); 
ros _inliers.header.stamp = input.header.stamp; 
ind pub.publish(ros inliers); 


pel: :ExtractIndices<pecl::PointxXY4> extract; 
extract .setiInputCloud(cloud.makeShared()) ; 
extract .setIndices(inliers) ; 


extract.setNegative (false) ; 
extract.filter (cloud segmented) ; 


sensor msgs::PointCloud2 output; 
pcl::toROSMsg(cloud segmented, output); 
pel pub.publish (output) ; 


protected: 
ros::NodeHandle nh; 
ros::Subscriber pcl_ sub; 
ros::Publisher pcl pub, ind pub, coef pub; 


main(int argc, char **argv) 


ros::init (argc, argv, "pcl planar segmentation") ; 
cloudHandler handler; 


ros::spin(); 


return 0; 


读者 可 能 注意 到 ， 这 个 广播 的 主题 中 用 到 了 两 种 新 的 数据 类 型 。 正 如 它们 的 名 字 所 提示 的 ，ModelCoefficients 消 息 储 存 了 
一 个 数学 模型 的 系数 ，Pointindices 储 存 了 一 个 点 云 中 点 的 这 引 。 我 们 友 布 它们 作为 一 个 表示 提取 信息 的 替代 方式 ， 它 们 可 以 用 
来 与 原来 的 点 云 (pcl_downsampled) 结合 以 便 提 取 正 确 的 点 。 提 示 一 下 ， 这 可 以 通过 将 上 友 布 对 象 的 时 间 惟 与 原始 点 云 消息 的 
时 间 惟 设置 成 一 样 ， 并 使 用 ROS 消 息 滤波 来 实现 : 


pcl pub = nh.advertise<sensor msgs : :PointCloud2>("pcl_ segmented", 1); 
ind pub = nh.advertise<pcl msgs::PointIndices>("point indices", 1); 
coef pub = nh.advertise<pcl msgs: :ModelCoefficients>("planar coef", 
1); 


跟 之 前 一 样 ， 在 回调 立 数 中 ， 执 行 从 PointCloud2 消 息 到 点 云 类 型 的 转换 。 在 这 种 情况 下 ， 也 需要 定义 两 个 新 的 对 象 对 应 本 
地 的 ModelCoefficients 和 Pointlndices 类 型 ， 这 些 会 在 分 割 算法 中 用 到 : 


pel: :PointCloud<pcl::PointxXYZ> cloud; 
pcl::PointCloud<pcl::PointxYZ> cloud segmented; 


pcl: :fromROSMsg (input, cloud); 


pcl::ModelCoefficients coefficients; 
pel::PointIndices::Ptr inliers (new pcl::PointIndices())j; 


分 割 算 法 允许 我 们 定义 ModelType 和 MethodType， 前 者 是 期 望 匹 配 的 数学 模型 ， 后 者 是 要 用 到 的 算法 。 正 如 之 前 解释 
的 ,我 们 使 用 随机 抽样 一 致 算法 是 因为 它们 对 离 群 点 的 鲁 棒 性 好 。 这 个 算法 也 允许 我 们 定义 两 个 停止 准则 : 和 迭代 的 最 大 次 数 
(setMaxlterations) 和 到 模型 的 最 大 距离 (setDistanceThreshold) 。 有 了 这 些 参数 设置 ， 加 上 输入 的 点 云 ， 算 法 残 可 以 工 
(FS, BEAR ( 落 入 模型 内 的 点 ) 和 模型 的 系数 : 


pel: :SACSegmentation<pcl::PointXYZ> segmentation; 
segmentation. setModelType (pcl: :SACMODEL PLANE) ; 
segmentation.setMethodType (pcl::SAC RANSAC) ; 
segmentation.setMaxIterations (1000) ; 
segmentation.setDistanceThreshold(0.01); 
segmentation.setInputCloud(cloud.makeShared() ) ; 


segmentation.segment (*inliers, coefficients); 


下 一 步 是 转 损 并 上 友 布 内 点 和 模型 系数 。 跟 之 前 一 样 ， 转 换 时 通过 标准 函数 执行 ， 但 是 你 也 许 注意 到 转换 函数 的 命名 空间 和 签 
名 与 用 来 做 点 云 转换 的 非常 不 同 。 为 进一步 加 深 这 个 示例 ， 这 些 消息 也 包含 原始 点 云 的 时 间 戳 ， 以 便 将 它们 链接 到 一 起 。 这 也 多 
许 在 其 他 节点 上 使 用 ROS 消 息 的 滤波 器 来 创建 包含 链接 在 一 起 的 对 象 的 回调 函数 : 


为 了 创建 分 割 点 云 ， 从 点 云 中 提取 出 内 


pel msgs::ModelCoefficients ros coefficients; 

pel conversions: :fromPCL (coefficients, ros coefficients) ; 
ros coefficients.header.stamp = input.header.stamp; 

coef pub.publish(ros_ coefficients) ; 


pel msgs::PointIndices ros inliers; 

pcl conversions: :fromPCL(*inliers, ros _inliers) ; 
ros inliers.header.stamp = input.header.stamp; 
ind pub.publish(ros inliers) ; 


点 。 最 简单 的 方法 是 使 用 Extractlndices 对 象 ， 但 是 也 可 以 仅仅 通过 在 索引 中 进行 循环 ， 并 将 相应 的 点 放 到 一 个 新 的 点 云 中 
来 轻松 实现 : 


pel: :ExtractIndices<pcl::PointXYZ> extract; 
extract.setInputCloud(cloud.makeShared())j; 
extract.setIndices(inliers) ; 
extract.setNegative (false) ; 
extract.filter (cloud segmented) ; 


最 后 ， 将 分 割 的 点 云 转换 为 PointCloud2 消 息 类 型 并 及 布 它 : 


sensor msgs::PointCloud2 output; 
pcl::toROSMsg (cloud segmented, output) ; 
pcl pub.publish (output) 


转换 的 结果 可 以 通过 下 面 的 图 像 来 查看 : 原始 的 点 云 显示 为 日 色 ， 而 分 割 的 内 点 显示 为 青绿 色 。 在 这 个 示例 中 ， 最 大 的 平面 
被 提 取 为 地 板 。 它 通常 可 能 是 我 们 想 要 从 点 云 中 提取 的 主要 元 素 之 一 ， 因 而 这 是 非常 方便 的 。 


> @ Grid 

v £5 PointCloud2 

> ¥ Status: Ok 
Topic 
Selectable 
Style 
Size (m) 
Alpha 
Decay Time 
Position Tra... 


Color Transf... 


Queue Size 
Color 


/pcl_segmented 
g 


Flat Squares 
0,1 

1 

0 

XYZ 
FlatColor 


| E 125; 200; 200 


“: PointCloud2 图 


¥ Status: Ok 
Topic 
Selectable 
Style 

Size (m) 

Alpha 

Decay Time 
Position Tra... 
Color Transf... 
Queue Size 


/pcl_output 
区 

Flat Squares 
0,01 

1 

0 

XYZ 
FlatColor 

10 


Color 
Color to assign to every point. 
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本 章 我 们 探索 了 PCL 中 各 种 不 同 的 工具 、 算 法 和 用 来 与 ROSs 中 的 点 云 进行 交互 的 接口 。 读 者 可 能 会 注意 到 我 们 努力 将 所 有 的 
示例 链接 到 一 起 ， 以 深入 理解 如 何以 一 种 可 重用 的 方式 使 用 这 种 节点。 不 省 如 何 ， 考 虑 到 点 云 处 理 的 计算 成 本 ， 任 何 架构 设计 都 
不 可 避免 地 与 使 用 系统 的 计算 能 力 相关 。 


我 们 给 出 的 示例 的 数据 流 源 目 于 数据 产生 者 ， 它 们 融 是 pcl create 和 pcl_read。 然 后 进入 数据 滤波 器 pcl _ filter 和 
pcl downsampling。 进 行 滤波 后 ， 通 过 pcl planar segmentation、pcl partitioning 和 pcl matching 将 更 为 复杂 的 信息 提取 
出 来 。 最 后 ， 数 据 通过 pcl_ write 与 入 硬盘 或 者 通过 pcl_ visualize 可 人 秽 化 显示 。 


本 章 的 主要 目的 是 提供 清晰 上 且 简 洁 的 示例 来 展现 如 何 将 PCL 库 的 基本 功能 与 ROS 集 成 ， 其 中 的 一 些 功能 可 以 限制 为 消息 和 和 转 
换 了 国 数 。 为 了 完成 这 个 目标 ， 我 们 解释 了 用 于 对 点 云 进行 数据 处 理 的 基础 技术 和 通用 算法 ， 因 为 我 们 意识 到 这 些 知识 越 来 越 重要 
了 。 


PiE ”3D 建 模 与 仿真 


相 比 于 仿真 ， 虽 然 直 接 在 一 个 真正 的 机 器 人 上 进行 编程 能 够 给 我 们 最 佳 的 有 反馈， 并 且 也 更 加 令 人 激动 ， 但 是 并 不 是 每 个 人 都 
有 机 会 接触 到 真正 的 机 器 人 。 基 于 这 个 原因 ， 我 们 需要 对 整个 物理 世界 进行 建 模 与 仿真 。 


在 本 章 中 我 们 将 会 学 习 : 
- 创建 机 器 人 的 3D 模 型 


` 为 机 器 人 提供 运动 、 物 理 限 制 、 惯 性 和 其 他 物理 响应 


` 为 机 器 人 3D 模 型 添加 仿真 传感器 


` 在 仿真 环境 中 使 用 该 模型 


7.1 ”在 ROS 中 目 定 义 机 器 人 的 3D 模 型 


机 器 人 3D 模 型 或 部 分 结构 模型 主要 用 于 仿真 机 器 人 或 者 为 了 帮助 开发 者 简化 他 们 的 日 常 工作 ， 在 ROS 中 这 通过 URDDF 文 件 
实现 。 

标准 化 机 器 人 描述 格式 (URDF) 是 一 种 用 于 摘 述 机 器 人 、 其 部 分 结构 、 关 节 、 上 自由 度 等 的 XML 格 式 文 件 。 你 每 次 在 ROS 系 
统 中 看 到 3D 机 器 人 都 会 有 有 URDF 文 件 与 之 对 应 ,例如 PR2 (Willow Garage) 或 者 Robonaut (NASA) 。 在 下 面 的 小 节 中 我 们 
将 会 学 习 如 何 创建 这 种 文件 和 格式 以 定义 不 同 的 值 。 


7.2 创建 第 一 个 URDF 文 件 


我 们 将 在 下 面 小 节 中 建立 的 机 器 人 是 一 种 最 弟 见 的 移动 机 器 人 ， 它 有 四 个 轮子 、 一 个 市 有 抓 取 器 的 手臂。 


为 了 打 好 基础 ， 我 们 先 做 一 个 带 有 四 个 轮子 的 机 器 人 底座 。 在 chapter7 tutorials/robot1 description/ysrc 人 /urdf 文 件 夹 下 创 
建 一 个 新 文件 并 命名 为 robot1.urdf。 将 下 面 的 代码 复制 到 文件 中 ， 这 些 URDF 代 码 是 基于 XML 的 ， 这 种 格式 并 不 强制 缩 进 但 还 
是 建议 大 家 这 么 做 。 所 以 ， 建 议 大 家 使 用 一 个 支持 缩 进 的 编辑 器 或 者 找到 适当 插件 并 进行 配置 (例如 ， 比 较 好 的 是 Vim 中 
的 .vimrc 文 件 ) : 


<7xml version="1.0"?> 
<robot namé="Robot1"> 
<link name="base link"> 
evVisual> 
aqeometry> 
<box size="0.2 .3 .1"/> 


</qeometry> 


<origin rpy="0 0 0" xyz="0 0 0.05"/> 


ematerial name="white"> 
<color rqba="1 1 1 1"/> 
«/material> 
< /wisual> 
s/link» 


<link name="wheel 1"> 
evVisual> 
<qeometry> 


«cylinder length="0.05" radius="0.05"/>s 


</qeometry> 


<origin rpy="0 1.5 0" xyz="0.1 0.1 O"/> 


ematerial name="black"> 
<color rgba="06 6 60 1"/> 
«/material> 
< /vVWisual> 
«/link> 


¢link names "wheel 2"> 
<Vlsual> 
<qeometry> 


ecylinder length="0.05" radius="0.05"/s 


e / geometry» 
origin rpy="0 1.5 0" xyz="-0.1 0 
material name="black"/> 
</visual> 
«/link> 
clink name="wheel 3"> 
<Visual> 
<qgeometry> 
ecylinder length="0.05" radiues"0 
</geometry> 
origin rpy="0 1.5 0O" xyz="0.1 -0.1 
«material name="black"/> 
</wisual> 
«/link> 


<link name="wheel 4"> 
<eVisual> 
<geometry> 
ecylinder length="0.05" radius="0 


.1 O" > 


.O5"/> 


O08 fs 


.O5" > 


</geometry> 
Origin Ppy="0. 1.5. 0" xyvz="-0.1 -0.1 01> 
<material name="black"/> 
</visual> 
</link> 


<joint name="base to wheell" type="fixed"> 
<parent link="base link"/> 
<child link="wheel 1"/> 
<origin xyz="0 0 0O"/> 

</joint> 


<joint name="base to wheel2" type="fixed"> 
<parent link="base link"/> 
<child link="wheel 2"/> 
<origin xyz="0 0 0"/> 

2/7 01nt> 


<joint name="base to wheel3" type="fixed"> 
<parent link="base link"/> 
<child link="wheel 3"/> 
<origin xyz="0 0 0"/> 

</joint> 


<joint name="base to wheel4" type="fixed"> 
<parent link="base_link"/> 
<child link="wheel 4"/> 
<origin xyz="0 0 0"/> 


“Fs 7 SLi 
</robot> 
7.3 xacro 个 更 好 的 机 器 人 建 模 万 ; 友 


我 们 看 一 下 robot1_physics.urdf 文 件 的 大 小 。 它 使 用 了 314 行 代码 来 定义 机 器 人 。 想 稼 一 下 如 果 我 们 开始 添加 摄像 头 、 腿 或 
者 其 他 几何 部 件 ， 那 么 这 个 文件 将 会 急剧 增 大 ， 并 且 维 护 这 些 代码 将 会 变 得 非常 困难 。 


xacro 可 帮助 我 们 压缩 URDF 文 件 的 大 小 ， 并 且 增 加 文件 的 可 读 性 和 可 维护 性 。 它 还 允许 我 们 创建 模型 并 复 用 这 些 模 型 去 创 
建 相 同 的 结构 ， 如 更 多 的 手臂 和 腿 。 


为 了 开始 使 用 xacro， 我 们 需要 指定 一 个 命名 空间 ， 以 便 文件 能 够 正确 地 解析 。 例 如 ， 这 是 一 个 有 效 的 xacro 文 件 的 前 两 


<?xml version="1.0"?> 
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="robot1 _ 
xeaCro" Ss 


其 中 我 们 定义 了 模型 的 名 称 ， 在 本 例 中 是 robot1 xacro。 记 住 ， 文 件 的 扩展 名 必须 是 .xacro， 而 不 是 .urdf。 


74 在 ROS 中 仿真 


想 要 在 ROS 系 统 中 对 我 们 的 机 器 人 进行 仿真 ， 需 要 使 用 Gazebo。 


Gazebo (http://gazebosim.org/) 是 一 种 适用 于 复杂 室内 多 机 器 人 和 室外 环境 的 仿真 环境 。 它 能 够 在 三 维 环境 中 对 多 个 
机 器 人 、 传 感 器 及 物体 进行 仿真 ， 产 生 实 际 传感器 反馈 和 物体 之 间 的 物理 啊 应 。 


Gazebo 现 在 独立 于 ROS， 并 人 在 Ubuntu 中 以 独立 功能 包 安 疼 。 人 在 本 节 中 ， 你 会 学 习 如 何 使 用 之 前 创建 的 机 器 和 人 模型， 如何 
加 载 一 个 激光 雷达 传感器 和 摄像 头 ， 并 使 机 器 人 模型 像 真 的 机 器 人 一 样 移动 。 


7.5 ”本章 小 结 


对 于 学 习 机 器 人 的 朋友 来 说 ， 能 够 接触 到 实际 的 机 器 人 非常 有 趣 也 非常 有 帮助 ， 但 并 不 是 每 个 人 都 有 机 会 。 当 你 难以 接触 到 
实际 的 机 器 人 时 ， 仿 真 则 是 一 个 非常 棒 的 工具 。 一 个 算法 在 实际 的 机 器 人 上 运行 前 也 需要 先 测试 下 算法 的 行为 ， 所 以 便 有 了 仿真 
工具 。 


在 本 草 中 ， 你 学 会 了 如 何 创建 一 个 属于 自己 的 3D 机 器 人 模型 。 这 包括 对 添加 纹理 、 创 建 天 书 和 说 明 如 何 使 用 书 点 来 移动 机 
器 人 等 工作 的 指导 和 详细 解释 。 


然后 我 们 介绍 了 Gazebo， 这 是 一 个 能 够 加 载 机 器 人 的 3D 模 型 并 对 其 运动 和 环境 感知 进行 仿真 的 仿真 环境 。 这 个 仿真 环境 被 
ROS 社 区 广泛 使 用 并 且 它 能 够 在 仿真 环境 中 支持 很 多 种 真正 的 机 器 人 。 


概括 来 讲 ， 我 们 已 经 看 到 了 如 何 复 用 其 他 机 器 人 的 部 件 来 设计 我 们 自己 的 机 器 人 ， 特 别 是 我 们 已 经 包含 了 抓 取 器 并 添加 了 传 
感 器 ， 例 如 一 个 激光 雷达 和 一 个 摄像 头 。 


因此 ， 在 仿真 中 不 需要 完全 从 无 到 有 来 建造 一 个 机 器 人 ， 社 区 里 已 经 开 友 了 大 量 的 机 器 人 ， 你 可 以 下 载 代码 并 在 ROS 和 
Gazebo 中 运行 它们 ， 如 有 必要 也 可 以 修改 它们 。 


你 可 以 在 下 面 链 接 找到 ROS 所 支持 的 机 器 人 列表 http://www.ros.org/wiki/Robots。 关 于 Gazebo 的 教程 见 下 面 链 
接 http://gazebosim.org/tutorials。 


在 下 一 章 ， 我 们 将 学 习 abond 功 能 包 ， 例 如 SLAM 和 使 用 激光 实现 导航 。 


第 8 章 SMIREN] 


在 前 面 几 章 中 ， 我 们 介绍 了 如 何 创 建 目 己 的 机 器 人 、 添 加 传感器 和 执行 器 以 及 使 用 游戏 杆 或 键盘 驱动 它 在 虚拟 环境 中 移动 。 
在 本 草 中 ， 你 会 学 习 到 ROS 系 统 最 强大 的 特性 之 一 ， 它 能 够 让 你 的 机 器 人 目 主 导航 和 运动 。 


得 益 于 开源 社区 和 共享 的 代码 ，ROS 拥 有 了 大 量 用 于 导航 的 算法 。 


首先 在 本 草 中 ， 我 们 会 学 习 使 用 自己 的 机 器 人 配置 导航 功能 包 集 所 必需 的 步骤 。 在 下 一 章 中 ， 会 学 习 对 仿真 机 器 人 配置 和 启 
动 导航 功能 包 集 ， 给 定 目标 和 配置 参数 以 获得 最 优 结果 。 特 别 地 ， 我 们 将 会 在 本 章 中 介绍 以 下 内 容 : 


` 介绍 导航 功能 包 集 和 它们 的 强大 功能 


很 明显 这 是 ROS 中 最 重要 的 软件 包 之 一 。 


` 介绍 放 ， 说 明 一 个 物理 量 如 何 从 一 个 坐标 系 变换 到 为 一 坐标 系 。 例 如 一 个 传感器 示人 和 集 的 数据 或 一 个 执行 器 收 到 期 望 位 置 的 


。 人 是 一 个 保持 坐标 系 跟踪 的 库 。 


> 
> 


. 创建 一 个 激光 雷达 的 驱动 或 对 其 进行 仿真 。 
- 计算 并 发 布 里 程 计 (odometry) 数据 ， 以 及 Gazebo 是 如 何 提 供 结 果 的 。 
基础 控制 器 ， 包 括 如 何 为 你 的 机 器 人 创建 它 。 


- 用 ROS 执 行 SLAM (Simultaneous Localization And Mapping) 。 使 用 自己 的 机 器 人 在 环境 中 移动 时 ， 为 此 环境 构建 一 个 地 
图 。 使 用 导航 包 集 里 的 AMCL 算 法 为 机 器 人 在 此 地 图 中 进行 定位 。AMCL 是 用 于 2D 机 器 人 运动 的 概率 定位 系统 。 它 采用 了 自 适应 


蒙特 卡 罗 定 位 (adaptive Monte Carlo localization, AMCL) 方法 ， 此 方法 使 用 了 一 个 粒子 滤波 器 在 一 个 已 知 地 图 中 跟踪 机 器 人 的 位 


最 后 ， 你 能 够 使 用 导航 功能 包 集 的 定位 彰 法 在 地 图 中 定位 你 的 机 器 人 。 


8.1 ”ROS 导航 功 能 包 集 


为 了 能 够 理解 导航 功能 包 集 ， 你 应 该 把 它 看 做 一 套 使 用 机 器 人 传感器 和 测 距 功能 的 算法 ， 以 便 使 用 标准 的 消息 来 控制 机 器 
军 无 难度 地 移动 机 器 人 到 你 想 要 的 位 置 (不 会 产生 页 撞 或 者 卡 在 某 个 位 置 ， 或 者 丢失 控制 信号 ) 。 


> 


你 可 以 认为 这 个 功能 包 集 能 够 为 任何 机 器 人 所 用 。 几 乎 可 以 说 是 这 样 的 ， 但 是 有 时 也 需要 调整 一 些 配 置 文件 并 且 调 用 功能 
集 编 与 一 些 节点 。 


在 使 用 导航 功能 包 集 之 前 ， 我 们 的 机 器 人 还 要 满足 一 些 条 件 : 


:导航 功能 包 集 只 能 够 处 理 双 轮 差分 驱动 和 完全 轮 驱 动 的 机 器 人 。 机 器 人 的 形状 必须 是 方形 或 者 天 形 。 当 然 ， 双 足 机 器 人 也 
育 


可 以 使 用 它 做 些 事情 ， 如 机 器 人 定位 等 ， 只 要 你 不 去 横向 移动 机 器 人 就 行 。 
` 需要 机 器 人 能 发 布 关 于 所 有 关节 和 传感器 位 置 关 系 的 信息 。 
. 机 器 人 必须 能 发 送 线 速度 和 角速度 消息 。 


. 机 器 人 必须 有 一 个 平面 激光 雷达 来 完成 地 图 构建 和 定位 。 或 者 ， 你 能 生成 一 些 相当 于 激光 雷达 或 者 声呐 的 数据 ， 如 果 它 们 
安装 在 机 器 人 的 其 他 地 方 ， 也 可 以 将 它们 的 值 映 射 到 地 面 上 。 


下 图 展示 了 导航 功能 包 集 的 组 织 方 式 。 在 图 中 ， 你 能 够 看 到 白色、 灰色 和 虚线 三 种 框 。 日 框 表示 其 中 的 这 些 功 能 包 集 已 经 在 
ROS 中 集成 了 ， 并 且 它 们 提供 的 多 种 节操 能 够 为 你 的 机 器 人 实现 目 主 导航 
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在 下 面 几 节 中 ， 我 们 还 会 看 到 如 何 创建 上 图 中 灰色 框 表示 的 内 容 。 这 些 部 分 取决 于 你 所 使 用 的 机 器 人 平台 。 换 句 话说 束 是 ， 
这 些 部 分 的 代码 并 不 是 通用 的 ， 需 要 根据 你 所 使 用 的 机 器 人 编写 一 定 的 代码 实现 ROS 和 导航 功能 包 集 的 数据 接口 。 


8.2 ”创建 变换 


导 肌 功能 包 集 需要 知道 传感器 、 轮 子 和 关节 的 位 置 。 


我 们 使 用 tf (Transform Frame) 软件 库 来 完成 这 部 分 工作 。 它 会 管理 坐标 变换 树 。 你 可 以 使 用 数学 工具 来 完成 这 部 分 工 
作 ， 但 如 果 你 需要 计算 很 多 的 坐标 系 ， 那 么 这 就 会 显得 有 些 复杂 和 混乱 了 。 


要 感谢 tf 软件 库 ， 使 得 我 们 可 以 向 机 器 人 添加 更 多 的 传感器 和 组 件 ，tf 会 为 我 们 处 理 这 些 设备 之 间 的 关系 。 


如 果 将 激光 雷达 向 后 移动 10cm 或 者 向 前 移动 20cm (相对 于 在 base_link 坐 标 系 中 的 原始 坐标 位 置 ) ， 我 们 都 需要 添加 一 个 
市 有 这 些 偏 移 的 新 坐标 系 到 坐标 变换 树 。 


一 旦 插入 和 创建 完成 ， 我 们 就 能 轻松 地 知道 激光 雷达 相对 于 base_link 值 或 者 是 相对 于 轮子 的 位 置 。 我 们 唯一 需要 做 的 就 是 
调用 tf 库 并 进行 坐标 变换 。 


8.3” 友 布 传感器 信息 


你 的 机 器 人 可 以 有 很 多 个 传感器 来 感 若 世界， 你 可 以 编写 很 多 节点 来 处 理 这 些 数 据 并 实现 一 些 功能 ， 但 是 导航 功能 包 集 仪 支 
持 使 用 平面 激光 雷达 传感器 。 所 以 你 的 传感器 必须 使 用 以 下 格式 友 布 数据 : sensor_msgs/LaserScan 或 


sensor msgs/PointCloud2, 


我 们 将 要 在 Gazebo 中 使 用 位 于 机 器 人 前 部 的 激光 雷达 进行 导航。 记 住 这 个 激光 雷达 是 在 Gazebo 中 仿真 出 来 的 ， 它 在 
hokuyo link 坐 标 系 中 以 /robot/lasescan 为 主题 名 发 布 数据 。 


如 果 使 用 我 们 原 有 的 机 器 人 模型 ， 那 么 不 需要 再 进行 任何 配置 束 能 够 在 导航 功能 包 集 中 使 用 我 们 的 激光 雷达 了 。 这 是 因为 我 
们 在 .urdf 文 件 中 配置 过 坐标 变换 树 ， 而 且 激 光 雷 达 也 能 够 使 用 正确 的 格式 友 布 数据 。 


如 果 你 使 用 了 一 个 真实 的 激光 雷达 ，ROS 很 可 能 已 经 包含 了 它 的 驱动 。 请 参考 第 4 草 中 如 何在 ROS 中 使 用 Hokuyo 激 光 雷 达 
的 相关 内 容 。 但 你 还 是 有 可 能 用 到 一 个 ROS 没 有 提供 官方 驱动 的 流光 雷达 ， 这 时 你 需要 自己 编写 一 个 忆 点 来 友 布 传感器 数据 ， 其 
数据 格式 需要 是 sensor_msgs/LaserScan。 人 在 下 一 书 中 我 们 会 专门 介绍 相关 功能 的 实现 ， 并 为 你 提供 一 个 示例 模板 。 


首先 请 你 记 住 消息 的 类 型 是 sensor_msgs/LaserScan。 使 用 以 下 命令 : 


$ rosmsg show sensor msgs/LaserScan 


随后 将 产生 以 下 输出 : 


std msgs/Header header 

uint32 seq 

time stamp 

string frame id 
float32 angle min 
float32 angle max 
Float32 angle increment 
float32 time increment 
float32 scan time 
float32 range min 
float32 range max 
float32[] ranges 


float32[] intensities 


创建 激光 雷达 市 点 


现在 我 们 在 chapter8 tutorials/src 文 件 夹 下 以 laser.cpp 为 名 创建 一 个 新 的 文件 ， 并 加 入 以 下 代码 : 


#include <ros/ros.h> 
#include <sensor msgs/LaserScan.h> 


int main(int argc, char** argv) { 
ros::init (argc, argv, "laser_scan_publisher") ; 


ros: :NodeHandle n; 


ros::Publisher scan pub = n.advertise<sensor _ 
msgs::LaserScan>("scan", 50); 
unsigned int num readings = 100; 


double laser frequency = 40; 
double ranges [num readings] ; 
double intensities [num readings]; 


int count = 0; 
ros::Rate r(1.0); 


while (n.ok()) { 
//generate some fake data for our laser scan 


for(unsigned int i = 0; i < num readings; ++i) { 
ranges[i] = count; 
intensities[i] = 100 + count; 

ros::Time scan_time = ros::Time: :now() ; 


//populate the LaserScan message 
sensor msgs::LaserScan scan; 


scan.header.stamp = scan time; 

scan.header.frame id = "base link"; 

scan.angle min = -1.57; 

scan.angle max = 1.57; 

scan.angle increment = 3.14 / num readings; 

scan.time increment = (1 / laser frequency) / (num readings); 
scan.range min = 0.0; 

scan.range max = 100.0; 


scan.ranges.resize(num_ readings) ; 
scan.intensities.resize(num_ readings); 


for(unsigned int i = 0; i < num readings; ++i) { 
scan.ranges[i] = ranges [i]; 
scan.intensities[i] = intensities [1]; 


| 


scan _pub.publish (scan); 
++count; 
r.sleep(); 


} 
| 


正如 你 所 见 ， 我 们 将 要 以 scan 为 名 创建 一 个 新 的 主题 ， 其 消息 格式 为 sensor msgSs/LaserScan。 你 应 该 对 这 种 消息 类 型 比 
较 熟 悉 ， 它 在 第 4 章 出 现 过 。 主 题 的 名 称 必 须 是 唯一 的 。 当 你 配置 导航 功能 包 集 时 ， 需 要 选择 这 个 主题 来 进行 导航 。 以 下 代码 显 


示 了 如 何 使 用 正确 的 名 称 创建 主题 : 


ros::Publisher Scan pub = n.advertise<sensor msgs: :LaserScan>("scan 


50) ; 

你 必须 要 友 布 header、stamp 和 frame id 等 字段 的 数据 。 因 为 要 想 让 导航 功能 包 集 运行 起 来 ， 必 须 有 
scan.header.stamp = Scan time; 
scan.header.frame_id = "base link"; 


" 
r 


这 些 数 据 的 支持 : 


在 header 中 的 frame_id 字 段 也 非常 重要 。 它 必须 是 在 .urdf 文 件 中 已 经 创建 好 的 坐标 系 之 一 ， 且 其 坐标 系 相关 数据 也 已 经 友 
布 到 tf 坐标 变换 中 。 导 航 功 能 包 集 会 通过 这 些 信息 获取 传感器 的 真实 位 置 并 进行 变换 ， 例 如 在 数据 传感器 和 障碍 物 的 坐标 系 之 间 


的 变换 。 


通过 使 用 这 个 示例 模板 ， 你 可 以 使 用 任何 激光 雷达 ， 而 无 需 考虑 ROS 中 是 否 有 它 的 驱动 。 你 只 需要 用 你 


据 来 奉 代 模板 中 的 默认 数据 。 


永 激光 雷达 上 的 真实 数 


这 个 模板 还 能 用 于 将 其 他 传感器 通过 数据 格式 转换 伪 北 成 一 个 激光 雷达 。 例 如 ， 你 能 够 将 双 目 视 党 测 距 系统 或 声呐 传感器 模 


拟 成 一 个 激光 雷达 。 


导航 功能 包 集 还 需要 获取 机 器 人 的 里 程 信息 。 里 程 信息 指 的 是 机 器 人 相对 于 某 一 点 的 距离 。 在 我 们 的 示例 中 ， 它 应 该 是 从 
base link 坐 标 系 原点 到 odom 坐 标 系 原 点 的 距离 。 


导航 功能 包 集 使 用 的 消息 类 型 是 nav_msgs/Odometry。 我 们 可 以 使 用 以 下 指令 查看 消息 的 数据 结构 : 


$ rosmsg show nav msgs/Odometry 


将 产生 以 下 输出 : 


std msgs/Header header 
uint32 seq 
time stamp 
string frame id 
string child frame id 
geometry msgs/PoseWithCovariance pose 
geometry msgs/Pose pose 
geometry msgs/Point position 
float64 x 
float64 y 
float64 z 
geometry msgs/Quaternion orientation 
float64 x 
float64 y 
float64 z 


Eloat64 w 
float64[36] covariance 


geometry msgs/TwistWithCovariance twist 
geometry msgs/Twist twist 

geometry msgs/Vector3 linear 
float64 x 
float64 y 
float64 z 

geometry msgs/Vector3 angular 
float64 x 
float64 y 
float64 z 


float64[36] covariance 


正如 在 消息 结构 中 所 见 ，nav _ msgsOdometry 提 供 了 从 机 器 人 frame id 坐标 系 到 child frame id 坐标 系 的 相对 位 置 。 它 还 
通过 geometry msgs/Pose 消 息 提 供 了 机 器 人 的 位 姿 信息 ， 通 过 geometry msgs/Twist 消 息 提 供 了 速度 信息 。 


位 次 信息 中 包含 着 两 个 结构 ， 一 个 显示 了 欧 拉 坐标 系 中 的 位 置 ， 另 一 个 则 使 用 了 一 个 四 元 数 显示 了 机 器 人 的 方向 。 机 器 人 的 
方向 也 是 机 器 人 的 角 位 移 。 


速度 信息 包含 了 两 个 结构 ， 一 个 是 线 速度 ， 另 一 个 是 角速度 。 对 于 我 们 的 机 器 人 ， 我 们 经 党 使 用 的 是 线 速 度 x 和 角速度 z-。 使 
用 线 速 度 x 是 为 了 知道 机 器 人 在 同 前 移动 还 是 在 同 后 移动 。 使 用 角速度 z 是 为 了 知道 机 器 人 在 向 左 转 还 是 向 右 转 。 


因为 里 程 其 实 束 是 两 个 坐标 系 之 间 的 位 移 ， 那 么 我 们 束 有 必要 友 布 两 个 坐标 系 之 间 的 坐标 变换 信息 。 我 们 在 这 里 已 经 对 最 后 
一 个 点 进行 了 里 程 变 换 ， 然 后 还 会 在 本 书 的 后 续 部 分 继续 介绍 如 何 发 布 里 程 数据 以 及 机 器 人 的 坐标 变换 树 信息 。 


现在 ， 让 我 们 看 看 Gazebo 是 如 何 处 理 里 程 信息 的 。 


8.5 ”创建 基础 控制 器 


对 于 导航 功能 包 集 来 说 ， 一 个 基础 控制 器 是 非常 重要 的 ， 因 为 这 是 唯一 能 够 有 效 地 控制 机 器 人 的 万 法 。 它 能 够 直接 和 你 的 机 
器 人 的 电子 设备 通信 。 


ROS 并 不 提供 任何 标准 的 基础 控制 器 ， 因 此 你 必须 目 己 编写 针对 你 移动 平台 的 基础 控制 器 。 

你 的 机 器 人 是 通过 geometry_msgs/Twist 类 型 的 消息 控制 的 。 这 个 类 型 正 是 我 们 之 前 看 到 的 Odometry 消 四 所 使 用 的 。 
所 以 你 的 基础 控制 器 必须 订阅 以 cmd_vel 为 名 称 的 主题 ， 必 须 生成 正确 的 线 速度 和 角速度 命令 来 驱动 平台 。 

现在 我 们 先 复习 一 下 消息 的 结构 。 在 命令 行 窗口 内 输入 以 下 命令 查看 消息 的 具体 结构 : 


$ rosmsg show geometry msgs/Twist 


这 个 命令 的 输出 结果 如 下 所 示 : 

geometry msgs/Vector3 linear 
float64 x 
float64 y 
float64 z 

geometry msgs/Vector3 angular 
float64 x 
float64 y 
float64 z 


其 中 ， 线 速度 向 量 linear 包 含 了 x、y 和 z 轴 的 线 速 度 。 角 速度 向 量 angular 包 含 了 各 个 轴 向 的 角速度 。 


对 于 我 们 的 机 器 人 ， 只 需要 使 用 线 速 度 x 和 角速度 z-。 这 是 因为 我 们 的 机 器 人 基于 医 分 轮 驱 动 平 台 ， 驱 动 它 的 两 个 电动 机 只 能 
够 让 机 器 人 前 进 、 后 退 或 者 转 癌 。 


我 们 先 在 Gazebo 中 对 这 个 机 器 人 进行 仿真 ， 基 础 控制 器 在 驱动 中 实现 ， 用 于 机 器 人 的 运动 和 仿真 。 这 也 束 是 说 企 Gazebo 
中 我 们 并 不 必 为 机 器 人 实际 创建 一 个 基础 控制 器 。 


\ 一 /一 


在 本 节 中 ， 你 还 会 学 到 实现 一 个 实际 物理 机 器 人 的 基础 控制 器 的 示例 。 在 此 之 前 ， 先 在 Gazebo 中 运行 我 们 的 机 器 人 ， 来 理 
解 基础 控制 器 的 作用 。 在 一 个 新 的 命令 行 窗口 中 运行 以 下 命令 : 


~ 


$ roslaunch chapter8 tutorials gazebo xacro.launch model:=" rospack find 
robotl description /urdf/robotl base 04.xacro" 
$ rosrun teleop twist keyboard teleop twist keyboard.py 


在 所 有 节点 都 局 动 和 正常 运行 之 后 ， 我 们 使 用 节点 状态 图 命令 rxgraph 来 查看 各 个 节点 之 间 的 天 系 : 


$ rqt graph 


rqt_graph__RosGraph - rqt 
Node Graph 
Lc) 


@ Group namespaces @ Group actions @ Hide dead sinks @ Hide leaf topics @ Hide Debug | @ Highlight @& Fit 


teleop_twist_keyboard 


/teleop_twist_keyboard 


joint_state_publisher robot_state_publisher 


/joint_state_publisher /joint_states /robot_state_publisher 


你 能 看 到 Gazebo 目 动 订阅 了 由 远程 操作 证 点 生成 的 cmd_vel 主 题 。 


在 Gazebo 仿 真 环境 中 ， 正 运行 看 我 们 的 差分 轮 陈 机 器 人 仿真 程序 插件 。 仿 真 程序 插件 通过 cmd _vel 主 题 获取 控制 售 令 数 
据 。 同 时 ， 仿 真 程序 插件 在 Gazebo 环 境 中 移动 机 器 人 并 生成 里 程 信息 。 


8.6 使 用 ROS 创 建 地 图 


创建 地 图 在 有 些 时 候 是 非常 复杂 的 工作 。 因 为 你 必须 选择 正确 的 工具 来 简化 你 的 工作 。ROS 就 有 这 样 的 工具 能 够 帮助 你 使 用 
激光 雷达 和 机 器 人 的 里 程 信息 创建 地 图 。 这 个 工具 是 map_server 地 图 服务 器 (http://wiki.ros.org/map_server) 。 在 本 示例 
中 ， 你 将 会 学 到 如 何 使 用 我 们 在 Gazebo 中 创建 的 机 器 人 来 创建 、 保 存 和 加 载 地 图 。 


我 们 将 会 使 用 一 个 .launch 文 件 来 简化 创建 的 过 程 。 在 chapter8 tutorialslaunch 文 件 夹 下 以 
gazebo_mapping_robot.launch 为 名 称 创建 一 个 新 文件 ， 然 后 添加 以 下 代码 : 


<?xml version="1.0"?> 
<launch> 

<param name="/use sim time" value="true" /> 

<include file="$(find gazebo ros) /launch/willowgarage world. 
launch" /> 

<arg name="model" /> 

<param name="robot description" command="$(find xacro) /xacro.py 
S(arg model)" /> 

<node name="joint state publisher" pkg="joint state publisher" 
type="joint state publisher" ></node> 

<!-- start robot state publisher --> 


<node pkg="robot state publisher" type="robot state publisher" 
name="robot_ state publisher" output="screen" > 


<param name="publish frequency" type="double" value="50.0" /> 
</node> 
<node name="spawn robot" pkg="gazebo ros" type="spawn model" 
args="-urdf -param robot description -z 0.1 -model robot model" 
respawn="false" output="screen" /> 
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find chapters 
tutorials) /launch/mapping.rviz"/> p 
<node name="slam gmapping" pkg="gmapping" type="slam gmapping"> 
<remap from="scan" to="/robot/laser/scan"/> 
<param name="base_link" value="base_footprint"/> 
</node> 
</launch> 


有 了 这 个 .launch 文 件 ， 你 就 能 够 以 3D 模 型 启动 我 们 所 需 的 Gazebo 仿 真 环境 。 在 这 个 仿真 环境 中 ， 你 能 够 通过 rviz 配 置 文件 
和 slam_mapping 来 实时 构建 地 图 。 在 命令 行 窗口 中 运行 这 个 局 动 文件 ， 并 在 另 一 个 命令 行 窗口 中 运行 远程 操作 节点 来 移动 机 器 
A 


$ roslaunch chapters tutorials gazebo mapping robot.launch 
model:=""rospack find robotl description /urdf/robotl base 04.xacro" 


$ rosrun teleop twist keyboard teleop twist keyboard.py 


当 你 使 用 键盘 移动 机 器 人 的 时 候 ， 你 会 在 rviz 屏 幕 上 看 到 很 多 空 日 和 未 知 的 空间 ， 也 有 一 部 分 空间 已 经 被 地 图 覆 兰 。 这 部 分 
CAE RIA E SHEA (Occupancy Grid Map, OGM) 。 每 当 机 器 人 移动 时 ，slam_mapping 证 点 都 会 更 新 地 图 
状态 。 或 者 更 详细 一 点 说 ， 在 机 器 人 移动 之 后 ， 将 会 得 到 机 器 人 位 置 更 好 的 估计 和 地 图 的 样子 。 将 会 使 用 流光 扫 拉 和 里 程 计 来 构 
建 OGM。 


mapping.rviz*® - RViz 


在 本 章 中 ， 你 逐步 学 习 了 使 用 导航 功能 包 集 之 前 ， 需 要 为 你 的 机 器 人 进行 的 配置 工作 。 介 绍 了 应 用 导航 功能 包 集 对 机 器 人 平 
人 台 的 要 求 ， 需 要 使 用 平面 激光 雷达 、 差 分 轮 式 机 器 人 、 基 础 控制 器 ， 而 且 还 要 满足 特定 的 几何 形状 。 


请 记 住 ， 我 们 使 用 Gazebo 来 展示 示例 和 解释 不 同 配置 下 导航 功能 包 集 的 工作 原理 。 如 果 直 接 使 用 一 个 真 的 机 器 人 平台 来 解 
释 这 一 切 束 会 打 烦 许多 ， 而 且 并 不 是 每 位 读者 都 能 拥有 或 者 能 接触 到 这 样 的 机 器 人 。 在 任何 情况 下 ， 使 用 不 同 的 机 器 人 平台 ， 所 
下 友 的 指令 都 可 能 是 不 同 的 ， 其 硬件 执行 器 也 可 能 会 出 现 故 障 ， 因 此 在 仿真 环境 中 执行 这 些 代码 更 加 安全 也 更 加 可 靠 。 在 此 之 
后 ， 在 满足 我 们 需要 的 基础 之 上 ， 我 们 也 可 以 将 这 些 代码 移植 到 实际 的 机 器 人 上 应 用 。 


在 下 一 章 中 ， 我 们 将 会 学 习 如 何 配置 导航 功能 包 集 ， 创 建 .launch 文 件 ， 并 在 Gazebo 中 完成 我 们 之 前 所 创建 的 机 器 人 的 自 


主导 航 。 


倘 单 来 说 ， 你 在 本 章 之 后 所 学 的 内 容 都 将 是 非常 有 用 的 ， 因 为 它 不 仪 会 指导 你 在 本 书 的 示例 中 进行 正确 的 配置 ， 还 会 指导 你 
在 其 他 的 机 器 人 (无论 是 仿真 机 器 人 还 是 真正 的 机 器 人 ) 中 使 用 导航 功能 包 集 。 


我 们 逐渐 接近 本 书 的 结尾 ， 而 本 章 将 对 本 书 所 学 的 所 有 知识 进行 综合 运用 。 我 们 已 经 创建 了 功能 包 、 节 点 、 机 器 人 的 3D 模 
型 等 。 人 在 第 8 章 中 ， 你 对 机 器 人 进行 了 配置 ， 以 便于 使 用 导航 功能 包 集 。 那 么 在 本 章 中 ， 我 们 将 会 完成 对 导 舰 功能 包 集 的 配置 ， 
这 样 你 束 能 够 学 会 如 何在 上 自己 的 机 器 人 上 使 用 它们 。 


在 前 面 章节 做 的 所 有 工作 都 成 了 现在 工作 的 铺垫 ， 而 最 大 的 乐趣 也 即将 开始 ， 这 是 赋予 我 们 的 机 器 人 生命 的 时 刻 。 
在 本 章 中 我 们 将 会 学 习 以 下 内 容 : 

应 用 在 第 8 章 所 学 的 知识 ， 并 继续 程序 开发 。 

理解 导航 功能 包 集 及 其 工作 方式 。 

` 配置 所 有 必要 文件 。 

+ 创建 启动 文件 并 开始 导航 。 


让 我 们 开始 吧 。 


9.1 创建 功能 
想 要 正确 创建 功能 包 ， 并 不 是 仅仅 把 文件 放 到 一 起 ， 而 是 要 为 我 们 机 器 人 创建 的 功能 包 添 加 依赖 项 。 例 如 ， 你 应 该 使 用 以 下 
命令 来 创建 功能 


$ roscreate-pkg my robot name 2dnav move base my tf configuration dep my 
odom configuration dep my sensor configuration dep 


但 是 在 我 们 的 示例 中 ， 已 经 把 所 有 代码 都 放 在 一 个 功能 包 中 了 ， 所 以 只 需要 执行 以 下 命令 : 


$ catkin create pkg chapter9 tutorials roscpp tf 


记 住 ， 在 ROS 的 软件 库 中 ， 你 也 可 能 找到 本 章 所 需要 的 所 有 文件 。 


9.2 创建 机 情人 配置 


为 了 一 次 局 动 整个 机 器 人 ， 我 们 将 会 创建 一 个 能 够 局 动 所 有 上 所 需 文 件 的 launch 局 动 文 件 。 无 论 如 何 ， 如 果 你 想 为 一 台 实 际 
的 机 器 人 编写 一 个 局 动 文 件 ， 你 可 以 用 下 面 的 文件 作为 模板 。 


下 面 的 代码 都 保存 在 configuration template.launch 文 件 中 : 


<launch> 
<node pkg="sensor node pkg" type="sensor node type" name="sensor _ 
node name" output="screen"> 
<param name="Sensor param" value="param value" /> 
</node> 


<node pkg="odom_ node pkg" type="odom_node type" name="odom_node" 
output="screen"> 
<param name="odom param" value="param value" /> 


</node> 


<node pkg="transform configuration pkg" type="transform 
configuration type" name="transform configuration name" 
output="screen"> 7 7 
<param name="transform configuration param" value="param value" /> 
</node> 
</launch> 


这 个 文件 会 通过 局 动 三 个 证 点 完成 机 器 人 的 启动 。 


第 一 个 书 点 用 于 负责 激活 传感器 如 激光 雷达 系统 (Laser Imaging Detection and Ranging, LIDAR) 的 初始 化 配置 。 参 数 
sensor_param 用 于 配置 传感器 的 端口 。 例 如 ， 有 的 传感器 会 使 用 USB 接 口 。 如 果 你 配置 的 传感器 要 更 多 的 参数 ， 那 么 你 需要 增 
加 相应 的 配置 行 和 必要 的 参数 。 有 些 机 器 人 具有 能 够 用 于 导航 功能 包 集 的 多 个 传感器 。 在 这 样 的 情况 下 ， 你 可 以 为 这 些 传感器 添 
加 更 多 的 节点 ， 并 在 创建 launch 启 动 文 件 时 ， 将 必要 的 节点 和 工具 都 包含 在 启动 文件 中 。 从 个 人 经 验 上 讲 ， 在 同一 个 文件 中 管 


理 这 些 节 后 会 比较 清晰 ， 能 够 避免 遗漏 和 错误 。 


第 二 个 节点 用 于 实现 里 程 数据 、 基 础 控制 器 、 机 器 人 移动 和 定位 等 必需 的 功能 。 在 第 8 章 中 ， 我 们 学 习 过 这 些 内 容 。 正 如 在 
其 他 小 书 一 样 ， 如 在 其 他 部 分 中 ， 你 可 以 使 用 参数 来 配置 里 程 计 ， 或 复制 行 以 添加 更 多 的 匡 点 。 


第 三 个 需要 启动 的 节点 主要 负责 发 布 和 计算 机 器 人 的 几何 结构 ， 各 个 手 避 、 传 感 器 之 间 的 坐标 变换 等 。 
前 面 的 文件 主要 是 在 你 使 用 真实 机 器 人 时 使 用 ， 而 对 于 我 们 现在 的 示例 ， 下 面 这 个 局 动 文件 包括 了 所 有 我 们 需要 的 。 


在 chapter9 tutorials/launch 文 件 夹 下 以 chapter9 configuration gazebo.launch 为 名 创建 一 个 新 文件 ， 并 添加 以 下 代 
45: 


<?xml version="1.0"?> 
<launch> 

<param name="/use sim time" value="true" /> 

<remap from="robot/laser/scan" to="/scan" /> 

<!-- start up wg world --> 

<include file="$(find gazebo ros) /launch/willowgarage world. 
launch"/> 

<arg name="model" default="$(find robot1 description) /urdf/ 


robot1 base 04.xacro"/> 


<param name="robot description" command="$ (find xacro) /xacro. 
py $(arg model)" /> 


<node name="joint state publisher" pkg="joint state publisher" 
type="joint state publisher" ></node> 
<!-- start robot state publisher --> 


<node pkg="robot state publisher" type="robot_ state publisher" 
name="robot state publisher" output="sScreen" /> 


<node name="spawn robot" pkg="gazebo ros" type="spawn_ model" 
args="-urdf -param robot description -z 0.1 -model robot model" 
respawn="false" output="screen" /> 

<node name="rviz" pkg="rviz" type="rviz" args="-d $(find 
chapter9 tutorials) /launch/navigation.rviz" /> 
</launch> 


这 个 文件 其 实 和 我 们 在 前 面 几 章 中 所 使 用 的 文件 完全 相同 ， 所 以 束 不 做 过 多 的 解释 了 。 
现在 使 用 以 下 命令 局 动 文件 : 


$ roslaunch chapter9 tutorials chapter9 configuration gazebo.launch 


你 会 看 到 如 下 窗口 : 


AAA  navigation.rviz* - RViz 


= [tinterct | nteract $a Move Camera [_]Select <$ Focus Camera Measure „~ 2D Pose Estimate A 2DNavGoal » 


v ® Global Options 
Fixed Frame map 
Background... MH 48; 48; 48 
Frame Rate 30 
v @ Global Statu... 
@ Fixed Fra... Fixed Frame [map] ... 
> @ Grid 
> © Static Map 
> & Particle Cloud 
> 到 Robot Foot... 
> -” Global Plan 
> -” Local Plan 
> -” Planner Plan 
>» “ Current Goal 
> J> TF 


| 口 网 网 网 网 网 网 网 网 


Fixed Frame 
Frame into which all data is transformed 
before being displayed. 


=| 
© 


ROS Time: ROS Elapsed: Wall Time: Wall Elapsed: [| Experimental 
30 fps 


请 注意 在 前 面 的 截屏 中 ， 有 一 些 字 段 在 你 没有 进行 任何 配置 时 ， 显 示 出 红色 、 赣 色 和 黄色 的 底 纹 。 这 是 因为 在 启动 文件 中 ， 
包含 了 一 个 rviz 布 局 的 配置 文件 。 这 个 文件 会 随 着 rviz 局 动 而 局 动 。 它 的 具体 配置 在 本 书 前 面 的 几 章 中 曾经 介绍 过 。 


在 下 面 的 章节 中 ， 我 们 会 学 习 如 何 配置 rviz 并 使 其 应 用 导航 功能 包 集 和 查询 所 有 的 主题 。 


好 的 ,现在 我 们 将 要 开始 配置 导航 功能 包 集 和 所 有 局 动 时 必需 的 文件 。 为 了 开始 进行 配置 ， 首 先 我 们 将 会 学 习 什么 是 代价 地 
图 和 代价 地 图 的 作用 。 我 们 的 机 器 人 将 使 用 两 种 导航 算法 在 地 图 中 移动 一 一 全 局 导航 (global) 和 局 部 导航 (local) 。 


` 全 局 导航 用 于 建立 到 地 图 上 最 终 目 标 或 一 个 远 距 离 目 标的 路 径 。 
` 局 部 导航 用 于 建立 到 近 距 离 目 标 和 为 了 临时 躲避 障碍 物 的 路 径 。 例 如 ， 机 器 人 四 周一 个 4X4/m 的 方形 窗户 。 
这 些 导航 算 去 通过 使 用 代价 地 图 来 处 理 地 图 中 的 各 种 信息 。 全 局 代价 地 图 用 于 全 局 导航 ， 局 部 代价 地 图 用 于 局 部 导航 。 


代价 地 图 的 参数 用 于 配置 算法 计算 行为 。 它 们 也 有 最 基本 的 通用 参数 ， 这 些 参数 会 保存 在 共享 的 文件 中 。 


我 们 可 以 在 三 个 最 基本 的 配置 文件 中 设 定 不 同 的 参数 。 这 三 个 文件 如 下 : 


e costmap_ common params .yaml 
e global costmap params .yaml 


e local costmap params .yaml 


你 只 需要 看 一 下 这 三 个 文件 的 名 称 ， 融 能 大 致 猜 出 来 它们 的 用 途 。 现 在 你 可 能 刚刚 对 代价 地 图 的 作用 有 一 个 急 步 的 认识 ， 让 
我 们 来 创建 这 些 配 置 文件 并 解释 这 些 配置 参数 的 作用 。 


94 ”为 导航 功能 包 集 创建 局 动 文 件 


现在 ,我 们 已 经 创建 和 配置 了 所 有 导航 功能 包 集 所 需 的 文件 。 为 了 运行 导航 功能 包 集 ， 我 们 还 要 创建 一 个 启动 文件 。 在 
chapter9 tutorials/launch 文 件 夹 下 创建 一 个 以 move base.launch 为 名 的 新 文件 ， 并 添加 以 下 代码 : 


<launch> 
<!-- Run the map server --> 


<node name="map server" pkg="map server" type="map server" 
args="$(find chapter9 tutorials) /maps/map.yaml" output="screen"/> 


<include file="$(find amcl) /examples/amcl diff.launch" /> 


<node pkg="move_ base" type="move_ base" respawn="false" name="move _ 
base" output="screen"> 


<rosparam file="$(find chapter9 tutorials) /launch/costmap_ common __ 
params.yaml" command="load" ns="global_costmap" /> 


<rosparam file="$(find chapter9 tutorials) /launch/costmap_ common __ 
params.yaml" command="load" ns="local_costmap" /> 


<rosparam file="$(find chapter9 tutorials) /launch/local_costmap_ 
params.yaml" command="load" /> 


<rosparam file="$(find chapter9 tutorials) /launch/global costmap 
params.yaml" command="load" /> 


<rosparam file="$(find chapter9 tutorials) /launch/base_ local _ 
planner params.yaml" command="load" /> 


</node> 
</launch> 


请 注意 ， 在 这 个 文件 中 我 们 会 局 动 之 前 创建 的 所 有 文件 。 我 们 还 会 局 动 一 个 市 有 第 8 草 中 创建 地 图 的 地 图 服务 器 和 amcl 忆 


我 们 将 会 使 用 的 这 个 amcl 忆 点 专用 于 支持 磊 分 驱动 机 器 人 平台 。 如 果 你 希望 用 它 驱 动 一 个 全 同 移动 机 嚣 人， 那么 你 束 需 
使 用 amcl_omni.launch 文 件 。 如 果 你 想 使 用 其 他 地 图 ， 那 么 请 参看 第 8 章 并 创建 一 个 地 图 。 


现在 局 动 文件 ， 并 在 一 个 新 的 命令 行 窗口 中 输入 以 下 命令 。 请 记 住 在 启动 这 个 文件 之 前 ， 你 需要 先 启 动 
chapter9 configuration gazebo.launch 文 件 。 


$ roslaunch chapter9 tutorials move base.launch 


然后 你 会 看 到 以 下 窗口 : 


@O navigation.rviz* - RViz 


- (interact | $ Move camera C] Select ”只 FocusCamera em Measure » 2DPoseEstimate »* 2DNavGoal @ PublishPoint 中 =y 


v ® Global Options 
Fixed Frame 
Background ... 
Frame Rate 

vv Global Statu... 
x Fixed Fra... 

> @ Grid 

> FE static Map 

> & Particle Cloud 

> W Robot Foot... 
> -” Global Plan 
> -” Local Plan 
> -” Planner Plan 
> A Current Goal 
> J) TF 

> ~ LaserScan 

> ih, RobotModel 

v Fl Map 

b te 


| 罗网 园 口 罗网 网 罗网 网罗 网关 


Remove Rename | 
© 


ROS Time ROS Elapsed wall Time: [1418860858.70 | wall Elapsed D Experimental 
30 fps 


如 果 你 比较 这 时 候 的 界面 与 之 前 你 启动 chapter9 configuration gazebo.launch 文 件 时 所 看 到 的 界面 ， 就 会 发 现 所 有 的 选 
项 都 变 成 蓝 色 了 ， 这 说 明 你 的 导航 功能 包 集 已 经 正常 启动 。 


正如 前 面 所 讲 ， 我 们 还 会 在 后 面 的 小 节 中 介绍 用 于 导航 功能 包 集 可 视 化 所 有 主题 必需 的 配置 选项 。 


9.5 ”为 导航 功能 包 集 设置 rviz 


通过 窗口 查看 导航 功能 包 集运 行 时 的 各 类 可 视 化 数据 对 于 学 习 导 航 功 能 包 集 来 说 非常 重要 。 在 本 节 中 ， 我 们 会 展示 如 何在 
rviz 中 查看 导航 功能 包 集 发 布 的 可 视 化 数据 ， 并 针对 导航 功能 包 集 友 布 的 每 一 个 可 视 化 数据 进行 介绍 。 


9.6 BEMS FRY EIM 


在 本 章 中 ， 我 们 将 会 使 用 目 适 应 蒙特 卡 罗 定 位 (AMCL) 算法 完成 机 器 人 定位 。 目 适应 蒙特 卡 罗 定 位 是 一 种 用 于 在 2D 环 境 
下 移动 机 器 人 的 概率 统计 定位 方法 。 这 种 算法 在 ROs 系 统 中 的 具体 实现 是 通过 在 已 知 地 图 的 基础 上 使 用 粒子 滤波 算法 跟 蹊 机 器 人 
AY. 


ROS 中 的 自 适应 蒙特 卡 罗 定 位 算法 在 系统 中 有 很 多 个 配置 选项 。 这 些 选 项 对 定位 算法 的 性 能 影响 较 大 。 如 果 你 想 了 解 该 算法 
的 具体 信息 ， 需 要 查看 AMCL 的 官方 文件 。 在 以 下 链接 中 ， 你 会 找到 关于 算法 的 更 详细 介绍 : 
及 http://www.probabilistic-robotics.org/。 


http://www.ros.org/wiki/amcl 以 


目 适 应 蒙特 卡 罗 定 位 节点 主要 使 用 激光 扫 摘 和 激光 雷达 地 图 。 当 然 它 可 以 通过 修改 代码 以 适应 其 他 类 型 的 传感器 数据 ， 例 如 
声呐 或 双 目 视 总 系统 。 对 于 本 章 ， 我 们 仅 使 用 激光 扫 摘 和 激光 和 雷达 生成 的 地 图 ， 传 递 消息 并 完成 位 次 估计 的 计算 。 我 们 要 移 针对 
ROS 系 统 提 供 的 各 个 初始 化 参数 ， 完 成 目 适 应 蒙特 卡 罗 定 位 算法 粒子 滤波 器 的 初始 化 。 如 果 你 没有 设 定 初 始 化 位 次 ， 目 适应 蒙特 
卡 罗 定 位 算法 会 假定 你 的 机 器 人 从 坐标 系 的 原点 开始 运行 ， 这 样 计算 会 相对 复杂 。 所 以 建议 你 在 rviz 中 通过 2D Pose Estimate 按 
钮 来 设 定 初始 化 位 姿 。 


然后 我 们 需要 引用 amcl_diff.launch 文 件 ， 并 调用 一 系列 的 配置 参数 来 启动 节点 。 这 些 配置 一 般 都 使 用 默认 配置 或 驱动 算法 
运行 的 最 小 设置 。 
下 面 ， 我 们 将 要 逐 项 学 习 amcl diff.launch 启 动 文件 的 内 容 ， 并 解释 其 中 的 各 个 参数 : 


<launch> 


<node pkg="amcl" type="amcl" name="amcl" output="sScreen"> 


<!-- Publish scans from best pose at a max of 10 Hz --> 


<param 


name="odom model type" value="diff"/> 


<param name="odom_alpha5" value="0.1"/> 
<param name="transform tolerance" value="0.2" /> 
<param name="gui publish rate" value="10.0"/> 


<param name="laser_max_ beams" value="30"/> 
<param name="min_ particles" value="500"/> 
<param name="max particles" value="5000"/> 


<param 


name="kld err" value="0.05"/> 


<param name="kld_z" value="0.99"/> 
<param name="odom alphal" value="0.2"/> 
<param name="odom_ alpha2" value="0.2"/> 
<!-- translation std dev, m --> 

<param name="odom_alpha3" value="0.8"/> 
<param name="odom alpha4" value="0.2"/> 
<param name="laser z hit" value="0.5"/> 


<param 
<param 


name="laser z short" value="0.05"/> 
name="laser z max" value="0.05"/> 


<param name="laser_z rand" value="0.5"/> 
<param name="laser sigma hit" value="0.2"/> 
<param name="laser_ lambda_short" value="0.1"/> 


<param name="laser lambda short" value="0.1"/> 
<param name="laser_ model type" value="likelihood_ field"/> 
<!-- <param name="laser model type" value="beam"/> --> 


<param 
<param 
<param 


<param 
<param 
<param 
<param 
<param 


</node> 
</launch> 


name="laser likelihood max dist" value="2.0"/> 
name="update min d" value="0.2"/> 
name="update min a" value="0.5"/> 


name="odom frame id" value="odom"/> 
name="resample interval" value="1"/> 
name="transform tolerance" value="0.1"/> 
name="recovery alpha _ slow" value="0.0"/> 
name="recovery alpha fast" value="0.0"/> 


其 中 ，min_particles 和 max_particles 参 数 设 定 了 算法 运行 所 允许 的 粒子 最 小 和 最 大 数量 。 如 果 你 使 用 更 多 的 粒子 ， 那 么 计 
算 结果 会 更 加 精确 ， 当 然 这 也 会 消耗 更 多 的 CPU 资源 。 


参数 laser model type 用 于 配置 激光 雷达 的 类 型 。 在 我 们 的 示例 中 ， 将 参数 设置 为 likelihood field。 但 在 算法 中 ， 也 可 以 
设置 为 beam 激 光 雷 达 。 


参数 laser likelihood max dist 用 于 设置 地 图 中 障碍 物 膨胀 的 最 大 距离 。 使 用 这 个 参数 的 前 提 条 件 是 前 面 选 择 了 
likelihood field 模 式 。 


参数 initial_pose_x、initial_pose_y 和 initial_pose_a 并 不 包含 在 局 动 文 件 中 。 但 可 以 通过 使 用 它们 设 定 在 算法 司 动 时 机 器 人 
的 切 始 位 置 。 例 如 ， 你 的 机 器 人 每 次 都 从 某 个 固定 的 充电 桩 局 动 ， 那 么 你 可 能 融会 在 局 动 文 件 中 去 设 定位 置 (而 无 需 在 rviz 中 设 
定 了 ) 。 


也 许 你 应 该 试 着 改变 启动 文件 中 的 一 些 参数 来 调试 机 器 人 ， 使 定位 算法 获得 最 佳 的 计算 结果 。 
在 http://wiki.ros.org/amcl 页 面 上 ， 你 能 够 找到 很 多 关于 这 些 参数 具体 配置 和 能 改变 的 信息 。 


9.7 ”使 用 rqt reconfigure 修 改 参 数 


通过 使 用 rqt_reconfigure 在 不 重新 局 动 仿真 的 情况 下 来 对 参数 值 进行 修改 ， 能 够 更 好 地 对 本 章 中 所 用 的 所 有 参数 进行 理 


oy 
十 d 
o 


使 用 以 下 指令 启动 rqt_reconfigure: 


$ rosrun rqt reconfigure rqt reconfigure 


你 将 看 到 以 下 窗口 : 


rqt_reconfigure_ Param - rqt 
Dynamic Reconfigure 


Filter key: 7 
Collapse al Ee 


amed 
gazebo 
> move_base 
» robot 


作为 示例 ， 我 们 将 对 base local planner _ params.yaml 文 件 中 的 max vel x 参数 进行 修改 来 演示 使 用 rqt_reconfigure 进 行 
参数 修改 的 过 程 。 单 击 上 图 中 的 “move _ base” 菜单 ， 展 开 该 菜单 项 。 然 后 在 菜单 树 下 选择 “TrajectoryPlannerROS”， 你 将 
看 到 一 个 参数 列表 。 如 你 所 见 ，max_vel x 参数 的 数值 和 我 们 在 配置 文件 中 设置 的 相同 。 

将 有 停 在 参数 名 字 上 几 秒 钟 ， 你 可 以 看 到 该 参数 的 一 个 简短 描述 ， 通 过 这 一 功能 你 
rqt_reconfigure_ Param - rqt 
# Dynamic Reconfigure 
Filter key: | 


| Collapseall ‘| Expand all 

I acc lim y 
amcl 

| gazebo 


acc_lim_theta 
| v move_base 


acc_lim_x 


TrajectoryPlannerROS 
> global_costmap 


> local_costmap 
| > robot 


max_vel_theta 


min_vel_theta 


9.8 HURSAN 


Siew eme SH — AREENAN AR, BEARER TENNARA. EGazeboh, RER 
在 机 器 人 前 面 放 一 个 物体 ， 你 融会 友 现 机 器 人 的 运动 友 生 改变 


。 例 如 ， 在 我 们 的 仿真 环境 中 添加 一 个 大 的 箱子 到 机 器 人 路 径 的 中 
央 。 导 舰 功 能 包 集 会 检测 到 新 的 障碍 物 并 目 动 创建 新 的 路 径 。 


在 下 图 中 ， 你 能 够 看 到 我 们 添加 了 一 个 物体 。Gazebo 有 一 些 预定 义 的 3D 物 体 。 它 们 可 以 在 仿真 中 与 移动 机 器 人 、 手 辟 
人 机 器 人 等 一 起 使 用 。 


Mk 


想 要 查看 这 些 物体 的 清单 ， 你 可 以 点 击 Insert model, 


选择 一 个 物体 并 点击 鼠标 选择 你 要 放置 的 位 置 ， 如 下 图 所 示 。 


如 果 你 在 rviz 窗 口中 ， 就 会 看 到 一 个 新 的 能 够 避 开 障碍 物 的 全 局 规划 。 如 果 你 有 一 个 真 的 机 器 人 ， 并 且 在 有 很 多 人 走 来 走 去 
的 环境 中 应 用 导航 功能 包 集 ， 那 么 你 会 友 现 这 个 功能 非 营 有 有趣。 如 果 机 器 人 探测 到 可 能 的 碰撞 ， 那 么 它 会 改变 运动 方向 ， 并 和 兰 试 
同 目 标 移动 。 你 能 够 在 下 图 中 清晰 地 看 到 这 个 功能 的 图 形 化 表示 。 记 住 对 这 些 障碍 物 的 检测 会 减 小 局 部 规划 器 代价 地 图 的 覆 草 学 
(例如 ， 机 器 人 周围 4x4/m 的 一 个 区 域 ) 。 
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我 相信 你 肯定 会 试看 在 地 图 上 移动 机 器 人 ， 看 看 它 到 旗 能 够 做 什么 。 但 是 每 次 都 是 差不多 的 结果 ， 可 能 慢 慢 地 你 融会 党 得 有 
AA T « 


TEM RA RES, WR CRW EE RIAS, NRRL AARE NEA. BIEN 
ABNER Frviz, BRST Aas). VIE, Were SS Re AactionlibxXSMWe. 


这 个 actionlib 功 能 包 为 这 样 的 功能 提供 了 标准 化 的 接口 。 例 如 ， 你 可 以 使 用 它 同 机 器 人 友 送 目标 坐标 来 检测 路 径 上 的 障碍 
物 ， 使 用 激 郊 雷达 扫 拉 等 。 在 本 节 中 ， 我 们 将 会 为 机 器 人 设 定 一 个 目标 并 等 待机 器 人 完成 这 项 工作 。 


这 和 服务 看 起 来 很 相似 ， 但 是 如 果 你 的 任务 要 持续 很 长 一 段 时 间 的 话 ， 那 么 你 可 能 想 要 在 命令 执行 过 程 中 取消 命令 或 者 获得 
当前 指令 执行 情况 的 定期 反馈 的 能 力 ， 这 些 功能 就 无 法 通过 服务 来 实现 了 。 更 进一步 说 ，actionlib 创 建 消息 (REARS) ME 
题 实现 具体 功能 。 这 样 我 们 可 以 通过 主题 来 友 送 目标 ， 并 在 需要 的 时 候 获 取 反 馈 或 执行 结 


下 面 的 代码 融 是 向 移动 机 器 人 肥大 目 标的 一 个 示例 。 在 chapter9 tutorials/src 文 件 夹 下 以 sendGoals.cpp 为 名 创建 一 个 新 
文件 ， 并 加 入 以 下 代码 : 


#include <ros/ros.h> 

#include <move base msgs/MoveBaseAction.h> 
#include <actionlib/client/simple action client.h> 
#include <tf/transform broadcaster.h> 


#include <sstream> 


typedef actionlib::SimpleActionClient<move base msgs: :MoveBaseAction> 


MoveBaseClient; 


int main(int argc, char** argv) { 
ros::init (argc, argv, "navigation goals"); 


MoveBaseClient ac("move_base", true); 


while(!ac.waitForServer (ros: :Duration(5.0))) { 


ROS INFO("Waiting for the move_base action server"); 


| 


move base msgs::MoveBaseGoal goal; 


goal.target pose.header.frame id = "map"; 
goal.target_pose.header.stamp = ros::Time: :now() ; 


goal.target pose.pose.position.x = 1.0; 
goal.target pose.pose.position.y = 1.0 


goal.target pose.pose.orientation.w = 1.0; 


ROS INFO("Sending goal"); 
ac.sendGoal (goal) ; 


ac.waitForResult (); 
if(ac.getState() == actionlib: :SimpleClientGoalState: : SUCCEEDED) 
ROS INFO("You have arrived to the goal position"); 


else{ 
ROS _INFO("The base failed for some reason"); 


} 


return 0; 


add executable (sendGoals src/sendGoals.cpp) 
target link libraries(sendGoals ${catkin LIBRARIES} ) 


在 你 的 CMakeList.txt 文 件 中 添加 以 下 文件 来 为 程序 编译 生成 可 执行 文件 : 


$ catkin make 


现在 ,使 用 以 下 命令 编译 功能 包 : 


$ roslaunch chapter9 tutorials chapter9 configuration gazebo.launch 
$ roslaunch chapter9 tutorials move base.launch 


下 一 步 就 是 使 用 启动 文件 启动 仿真 环境 并 测试 新 程序 。 使 用 下 面 的 指令 来 启动 所 有 节点 和 配置 


$ rosrun chapter9 tutorials sendGoals 
AMAD, (FBLA Rae“ ias ST I HF aasendGoalra: 


$ rosrun chapter9 tutorials sendGoals 


如 果 你 查看 rviz 屏 幕 ， 你 会 在 地 图 中 看 到 一 个 新 的 全 局 规划 ( 绿 线 ) 。 这 意味 背 导 航 功 能 包 集 已 经 接受 了 新 的 目标 并 开始 执 


/—\—— 一 
J 这 个 目标 。 
navigation.rviz* - RVIz 


‘(ne |< move camera jsSelect <@ Focus Camera em Measure ~” 2D Pose Estimate .~ 2DNavGoal  PublishPoint 4p 


ROS Time: [2051.36 | ROS Elapsed: [2051.36 | wall Time: C] Experimental 


SNesARABi, Resin Rasta Peel FAVES: 


[ INFO ] [...,...]: You have arrived to the goal position 


你 可 以 设置 一 系列 的 路 径 点 ， 并 帮助 你 的 机 器 人 规划 一 条 大 致 的 线路 。 这 样 你 融 能 够 编写 任务 程序 、 引 导 机 器 人 运送 物品 和 
查看 其 他 房间 的 情况 。 
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完成 本 章 的 学 习 之 后 ， 你 应 该 已 经 可 以 通过 导航 功能 包 集 驱动 你 的 机 器 人 了 。 无 论 你 使 用 的 是 真正 的 机 器 人 还 是 仿真 机 器 
人 ， 使 用 导航 功能 包 集 ， 你 的 机 器 人 残 可 以 在 地 图 中 目 主 移动 了 。 你 可 以 根据 ROS 代 码 复 用 的 理论 编写 控制 和 机 器 人 定位 算法 ， 
以 便 你 可 以 较 容易 地 完成 机 器 人 的 全 部 配置 。 本 章 中 的 难点 在 于 理解 各 个 配置 参数 并 学 会 如 何 使 用 这 些 参数 。 参 数 的 正确 使 用 决 
定 了 机 器 人 能 人 否 正音 工作 。 所 以 你 必须 检查 和 调整 参数 ， 看 看 机 器 人 的 反应 如 何 。 


在 下 一 章 中 ， 你 将 通过 一 些 教程 和 例子 学 习 如 何 使 用 Movelt! 。 可 能 你 不 熟悉 Movelt! ， 它 是 一 种 开发 移动 机 械 臂 的 软 
件 。 使 用 它 ， 可 以 很 简单 地 移动 关节 连接 的 机 器 人 。 


第 10 草 使 用 Movelt! 


ROS 中 有 针对 机 器 人 进行 移动 操作 的 一 套 工 具 一 一 Movelt! 。 在 主页 http://moveit.ros.org 上 包含 使 用 Movelt! 的 文档 、 
教程 、 安 装 说 明 以 及 多 种 机 械 辟 (或 机 器 人 ) 的 示例 演示 ， 如 一 些 移动 操作 任务 ， 包 括 抓 握 、 拾 取 和 放置 ， 或 简单 的 逆向 运动 学 
的 运动 规划 。 

这 个 库 包 合 一 个 快速 的 逆 运 动 学 解 算 器 (作为 运动 规划 单元 的 一 部 分 ) 、 先 进 的 操作 算法 、 三 维 感知 抓 握 〈 通 单 以 点 云 的 形 
式 ) 、 运 动 学 、 控 制 和 导航 等 功能 。 除 了 后 台 功 能 ， 它 还 提供 了 一 个 易于 使 用 的 图 形 用 户 界 面 (GUI) 通过 Movelt! 和 rviz 插 件 
配置 新 的 机 械 辟 ,使 用 户 能 以 直观 的 方式 进行 运动 规划 任务 的 开 友 。 


在 本 章 中 ， 我 们 将 学 习 如 何 使 用 URDF 格 式 创建 一 个 简单 的 机 械 臂 ， 以 及 使 用 Movelt! 配置 工具 定义 运动 规划 群 组 。 对 于 
每 个 机 械 辟 ,我 们 将 对 应 一 个 群 组 ， 之 后 束 可 以 使 用 逆 运 动 学 解 算 器 去 执行 由 rviz 接 口 设 定 的 操作 任务 。 最 后 通过 一 个 抓 取 和 帮 
置 任务 来 展示 Movelt! 的 功能 及 工具 的 使 用 。 

10.1 节 介绍 Movelt! 的 体系 结构 ， 解 释 这 套 框架 中 所 用 到 的 基本 概念 〈 如 关节 群 组 和 规划 场景 ) ， 以 及 一 般 性 的 概念 (如 
轨迹 规划 、“( 逆 ) 运动 和 碰撞 检测 等 ) 。 然 后 ， 将 介绍 如 何 将 机 醋 辟 集成 到 Movelt! 中 ， 创 建 规划 群 组 和 场景 。 接 下 来 ， 将 展 
示 如 何在 有 障碍 物 的 环境 中 进行 运动 规划 ， 以 及 如 何 结合 点 云 数据 ， 避 免 碰 撞 到 动态 障碍 物 。 


最 后 ,介绍 感知 和 目标 识别 工具 ， 然 后 将 其 应 用 于 抓 取 和 放置 示例 的 演示 。 人 在 这 个 示例 中 ， 我 们 将 在 rviz 使 用 Movelt! H 
件 。 


10.1 Movelt! 体系 结构 


Movelt! 的 体系 结构 如 图 1 所 示 ， 内 容 摘自 官方 文档 的 概念 部 分 (http://moveit.ros.org/documentation/concepts/) 。 


在 这 里 ， 我 们 简要 介绍 主要 概念 。 若 想 安 闭 Movelt! ， 只 需要 运行 这 个 命令 : 


$ sudo apt-get install ros-hydro-moveit-full 


另外 ， 也 可 以 通过 在 它 相 应 的 工作 空间 运行 下 面 的 命令 来 安装 本 章 附 市 代码 的 所 有 依赖 包 : 


$ rosdep install --from-paths src -iy 
ROS 参 数 服 务 器 

HPRH 
(C++) 抓 取 动 作 
| \ 放置 动作 
! moveit_commander \ DEB LEA EIR 
ET 
| \ 获取 FK 服 务 = 
| ”获取 规划 有 效 性 服务 
oe : 规划 路 径 服 务 p 
{GUL (rvizdli) | 执行 路 径 服务 : 
| 获取 规划 场景 服务 
| 其 他 接口 附属 对 象 
碰撞 对 象 
| 规划 场景 差别 


~weeseeweewrewee eww eweeewew w= @ @ @ @ = ” 


图 1 Movelt! 的 体系 结构 框图 


体系 结构 的 中 心 是 move_group 单 元 。 其 主要 思想 是 ， 首 先 需要 定义 由 关节 和 其 他 元 件 构成 的 群 组 (group) ， 然 后 使 用 运 
动 规划 算法 执行 移动 操作 。 这 类 算法 考虑 与 物体 交互 的 场景 以 及 该 群 组 关节 的 特性 。 

群 组 使 用 标准 的 ROS 工 具 和 定义 语言 进行 定义 ， 如 YAML、URDF 和 SDF 等 。 简 而 言 之 ， 我 们 必须 对 关节 进行 定义 ， 它 们 是 
一 个 群 组 的 部 分 并 包含 关节 约束 。 同 样 ， 可 定义 未 端 执行 器 的 工具 ， 如 一 个 夹 持 器 和 感知 传感器 。 机 器 人 必须 开放 
JointTrajectoryAction 控 制 器 ， 从 而 使 运动 规划 的 输出 可 以 在 机 器 人 的 硬件 (或 仿真 器 ) 规划 执行 。 为 了 监控 执行 情况 ， 需 要 通 
过 机 器 人 状态 发 布 者 发 布 /joint_states。 所 有 内 容 都 由 ROS 控 制 并 由 特定 传感器 驱动 发 布 。 需 要 注意 的 是 Movelt! 提供 了 一 个 
图 形 用 户 界面 向 导 去 定义 给 定 的 机 器 人 关节 群 组 ， 可 以 通过 下 面 的 命令 直接 调用 


$ roslaunch moveit setup assistant setup assistant.launch 


Smove_groupatsilifie, MAME. Movelt! 提供 了 C++ 和 Python 的 应 用 程序 接口 (API) 以 及 一 个 无 颖 
集成 的 rviz 插 件 来 实现 此 功能 ， 使 我 们 可 以 向 机 器 人 友 送 运动 目标 ， 规 划 任务 并 友 送 给 机 器 人 执行 等 ， 如 图 2 所 示 。 


10.2 在 Movelt! 中 集成 一 个 机 械 辟 


在 本 节 中 ， 我 们 将 通过 不 同 的 步骤 实现 在 Movelt! 中 使 用 一 个 机 械 辟 。 尽 管 一 些 内 容 将 在 本 章 后 面 讲 到 ， 但 仍然 需要 事先 
补充 几 个 基础 知识 ， 如 手臂 摘 述 文件 (URDF) ， 以 及 一 些 在 Gazebo 运 行 的 组 件 。 


10.3 ”简单 的 运动 规划 


rviz 插 件 提供 了 一 个 非常 有 趣 的 与 Movelt! 进行 交互 的 机 制 ， 但 由 于 它 缺 乏 目 动 化 ， 可 能 会 伞 认 为 相当 具有 局 限 性 甚至 很 麻 
烦 。 为 了 充分 友 挥 Movelt! 中 的 功能 ， 已 开 友 了 几 个 应 用 程序 接口 (API) ， 这 使 我 们 能 够 执行 一 系列 的 操作 ， 如 运动 规划 、 访 
问 机 器 人 的 模型 和 修改 规划 的 场景 等 。 


在 下 面 的 内 容 中 ， 我 们 将 通过 几 个 例子 介绍 如 何 执行 不 同类 型 的 简单 运动 规划 。 我 们 将 开始 规划 一 个 早 一 的 目标 ， 然 后 规划 
一 个 随机 目标 ， 接 看 进行 预定 义 群 组 的 状态 的 规划 ， 最 后 解释 如 何 改进 我 们 与 rviz 相 互 作用 的 代码 段 。 


为 了 简化 和 解释， 提供 了 一 组 启动 文件 启动 全 部 所 需 的 功能 。 其 中 最 重要 的 一 个 负责 局 动 Gazebo、Movelt! 和 机 械 辟 的 控制 


aa. 
$ roslaunch rosbook arm gazebo rosbook arm empty world.launch 
RMF S A TAAN EAD, AFlaatrvizihieoaxiatt. rvizixekiatt ee ith, (BIR RAR, EX 
一 节 将 进一步 使 用 : 


$ roslaunch rosbook arm moveit config moveit RViz.launch config:=true 


我 们 提供 一 定数 量 的 代码 段 ， 用 于 进一步 解释 说 明 。 代 码 段 放 在 rosbook arm_snippets 功 能 包 中 。 代 码 段 包 除 了 代码 外 不 
包含 其 他 任何 东西 ， 通 过 调用 rosrun 而 不 是 跟 往常 一 样 使 用 roslaunch 局 动 代码 段 。 


从 典型 的 ROS 初 始 化 开始 ， 本 节 中 每 个 代码 段 都 遵循 相同 的 模式 ， 本 章 不 绸 瑞 述 。 初 始 化 之 后 ， 我 们 需要 定义 对 那些 将 要 执 
行 运动 规划 的 规划 群 组 。 在 我 们 的 示例 中 只 有 两 个 规划 群 组 ， 机 械 臂 和 夹 持 器 ， 这 里 我 们 只 关注 臂 。 这 将 实例 化 一 个 规划 群 组 接 
口 ， 并 负责 与 Movelt! 进行 交互 ， 如 下 : 


moveit::planning interface: :MoveGroupPp plan group ("arm"); 
在 规划 群 组 接口 实例 化 后 ， 通 常用 一 些 代码 决定 规划 的 目标 ， 它 对 应 本 节 包 括 的 每 种 类 型 的 目标 。 在 目标 确定 后 ， 需 要 传达 
Movelt! 使 它 执 行 。 下 面 的 代码 段 负 责 创建 规划 和 使 用 规划 群 组 接口 去 请 求 Movelt! 执行 运动 规划 ， 如 果 成 功 ， 将 执行 : 


moveit::planning interface::MoveGroup::Plan goal plan; 
if (plan group.plan(goal plan) ) 


( 


plan group.move(); 


| 


10.4 考 夸 础 撞 的 运动 规划 


让 读者 知道 Movelt! 提供 了 考虑 人 础 撞 情 况 的 运动 规划 可 能 是 有 趣 的 ， 我 们 将 介绍 如 何 将 物体 添加 到 可 能 与 我 们 机 械 臂 友 生 
页 撞 的 规划 场景 中 。 首 先 ， 将 解释 如 何 将 基本 对 象 添加 到 规划 的 场景 中 ， 即 使 在 我 们 的 场景 中 不 存在 一 个 真正 的 物体 ， 它 也 允许 
我 们 执行 规划 。 这 是 相当 有 趣 的。 之 后 ， 将 介绍 如 何 从 场景 中 删除 这 些 物体 。 最 后 ， 将 解释 如 何 添加 RGBD 传 感 器 ， 它 会 基于 真 
实 环 境 (或 模拟 ) 的 对 象 产 生 点 云 ， 从 而 使 我 们 的 运动 规划 更 有 趣 和 实用 。 


10.5 ” 抓 取 和 放置 任务 


在 本 节 中 ， 我 们 要 解释 如 何 用 机 械 辟 执行 一 个 很 常见 的 应 用 程序 或 任务 。 抓 取 和 放置 任务 包括 拿 起 一 个 目标 对 铺 : 抓 住 它 ， 
并 放置 到 另 一 个 位 置 。 在 这 里 ， 我 们 假设 对 象 最 蕊 位 于 文 撑 面 上 ， 又 返 面 可 以 是 平面 或 曲面 ， 例 如 果子 ， 很 容易 推广 到 更 复杂 的 
环境 。 至 于 要 抓 取 的 对 象 ， 考虑 将 其 近似 为 方 爹 ， 这 样 当 使 用 夹 持 器 抓 取 时 就 非常 简单 。 对 于 更 复杂 的 对 象 ， 将 需要 更 好 的 夹 持 
器 ， 甚 全 一 只 机 械 手 。 


在 下 面 的 小 节 中 ， 我 们 首先 描述 如 何 设置 规划 场景 ， 这 是 Movelt! 除了 机 械 臂 本 身 外 还 需要 标识 的 对 象 。 这 些 对 象 在 运动 
规划 时 需要 考虑 避 障 ， 可 以 被 拿 起 或 者 抓 握 。 为 了 简化 问题 ， 我 们 将 省 略 感知 的 部 分 ， 但 我 们 将 解释 它 是 如 何 实现 和 集成 的 。 一 
旦 定义 了 规划 的 场景 ， 我 们 将 摘 述 如 何 使 用 Movelt! 的 应 用 程序 接口 完成 抓 取 和 放置 的 任务 。 最 后 ， 我 们 将 解释 如 何在 演示 模 
陈 下 ， 使 用 虚拟 控制 器 运行 此 任务 ， 这 样 我 们 残 不 需要 实际 的 机 器 人 系统 (不 论 在 Cazebo 中 或 在 一 个 真正 机 器 人 上 ) 。 我 们 还 
将 向 你 展示 当 仿 真 机 械 辟 与 环境 中 仿真 对 象 交 互 时 ， 如 何在 Gazebo 中 真正 看 到 仿真 机 械 辟 的 运动 。 


10.6 “本章 小 绪 


本 章 涵 蘑 了 将 一 个 机 械 句 集成 到 Movelt! 和 Gazebo 中 所 涉及 的 大 部 分 内 容 ， 讲 述 了 如 何在 真实 仿真 环境 中 使 用 机 械 芒 。 
Movelt! 为 我 们 提供 了 一 类 使 用 规划 求解 逆 运 动 学 模型 (IK) 进行 机 械 辟 运动 规划 的 非常 简 单 和 简洁 的 工具 ， 以 及 便于 使 用 的 
丰富 文档 。 但 鉴于 该 体系 结构 的 复杂 性 ， 只 有 当 正 确 理解 了 Movelt! 、 机 器 人 的 传感器 和 执行 器 之 间 所 有 不 同 的 相互 作用 时 ， 
才能 正常 工作 。 


我 们 已 经 了 解 了 Movelt! 应 用 程序 接口 中 不 同 的 高 级 别 对 象 ， 详 细 介 绍 需 要 涵盖 一 整 本 书 的 内 容 。 为 了 避免 在 理解 一 个 AP| 
如 何 完整 地 执行 非常 简 捍 的 操作 时 浪费 时 | 旧 ， 本 书 中 玉 取 的 办 法 是 一 直 把 内 容 局 限于 非常 简 蛙 的 运动 规划 以 及 在 规划 场景 人 工 创 
建 对 象 并 和 RGB-D 传 感 器 中 生成 点 云 进行 允 互 。 


最 后 ， 对 如 何 执行 目标 对 象 的 抓 取 和 放置 任务 提供 了 一 个 非常 详细 的 讲解 。 虽 然 这 不 是 机 械 辟 的 唯一 目的 ， 但 可 能 是 你 喜欢 
的 一 个 尝试 ， 并 且 是 非常 常见 的 工业 机 器 人 功能 。 使 用 Movelt! 运动 规划 和 3D 感 知 ， 可 以 在 复杂 动态 环境 下 实现 这 样 的 任务 。 
对 于 特定 用 途 的 机 械 辟 ,需要 对 Movelt! 的 体系 结构 和 APl 进 一 步 学 习 和 深化 ， 这 样 可 以 使 你 更 加 深入 地 理解 Movelt! 的 功 
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